#include "GRBLMaster.hpp"
#include "MiscUtils/TdrMisc/SEngine.h"
//#include "SEngineHeaders/Auxi.hpp"
//#include "SEngineHeaders/math.hpp"
#include <Windows.h>
#include "Comms/SerialPortManager/SerialPortManager.hpp"

using namespace std;

#define SlaveComm ((SerialPortManager*)_core.slaveNativeHandle)

// G10 L2 P0X0Y0    ->   set x=0,y=0

// move x=3
// G91G0X3

// set work coord
// G92X1Y2   ->   set x=1,y=2

const std::string GRBLParser::axisCodes = "XYZABCDEFGHIJKLMN";

const std::string GRBLParser::paraAbsPosMode = "G90";
const std::string GRBLParser::paraRltPosMode = "G91";
const std::string GRBLParser::paraMove = "G0";
const std::string GRBLParser::paraMoveWithSpeed = "G1";
const std::string GRBLParser::paraMoveSpeedOverwrite = "F";

const std::string GRBLParser::paraSetTempWorkPos = "G92";
const std::string GRBLParser::paraRemoveTempWorkPos = "G92.1"; // g92.2 disable but keep them, not supported here
const std::string GRBLParser::paraEnableHardLimit = "$21=1"; // enable hard limit
const std::string GRBLParser::paraDisableHardLimit = "$21=0";
const std::string GRBLParser::paraEnableHoming = "$22=1";
const std::string GRBLParser::paraStartHoming = "$H";
const std::string GRBLParser::paraDisableHoming = "$22=0";
const std::string GRBLParser::paraSetOffset = "G10 L2 P";
const std::string GRBLParser::paraSetXMaxSpeed = "$110=";
const std::string GRBLParser::paraSetYMaxSpeed = "$111=";
const std::string GRBLParser::paraSetZMaxSpeed = "$112=";
const std::string GRBLParser::paraSetXMaxAcc = "$120=";
const std::string GRBLParser::paraSetYMaxAcc = "$121=";
const std::string GRBLParser::paraSetZMaxAcc = "$122=";
const std::string GRBLParser::paraEnableHardLimitReport = "$10=19";

const std::string GRBLParser::stateMove = "Run";
const std::string GRBLParser::stateIdle = "Idle";
const std::string GRBLParser::stateAlarm = "Alarm";
const std::string GRBLParser::stateHoming = "Home";
const std::string GRBLParser::stateCmdReply = "ok";

const std::string GRBLParser::keyMachinePos = "MPos:";
const std::string GRBLParser::keyWorkPos = "WPos:";
const std::string GRBLParser::keyLimit = "Lim:";
const std::string GRBLParser::keyToUnlock = " to unlock]";
const std::string GRBLParser::keyHardLimitedIdle = "ALARM:1";
const std::string GRBLParser::keyHardLimitedRun = "ALARM:3";
//const std::string GRBLParser::keyHardLimited0 = "ALARM: Hard"; // not completed
//const std::string GRBLParser::keyHardLimited1 = "[Reset to continue]";
const std::string GRBLParser::keyUnlock = "[Caution: Unlocked]";

#define AXIS_MASKED(mask, index) ((mask & (0x00000001<<index)) == (0x00000001<<index))
#define X_MASKED(mask) ((mask & 0x0001) == 0x0001)
#define Y_MASKED(mask) ((mask & 0x0002) == 0x0002)
#define Z_MASKED(mask) ((mask & 0x0004) == 0x0004)
#define A_MASKED(mask) ((mask & 0x0008) == 0x0008)
#define B_MASKED(mask) ((mask & 0x0010) == 0x0010)
#define C_MASKED(mask) ((mask & 0x0020) == 0x0020)
#define D_MASKED(mask) ((mask & 0x0040) == 0x0040)
#define E_MASKED(mask) ((mask & 0x0080) == 0x0080)

float calProgress(float* start, float* current, float* target, int posLen) {
	float maxDiff = -1;
	int maxDiffIdx = -1;
	for (int idx = 0; idx < posLen; idx++) {
		float diff = abs(target[idx] - start[idx]);
		if (diff > maxDiff) {
			maxDiff = diff;
			maxDiffIdx = idx;
		}
	}

	if (maxDiff == 0) {
		return 1;
	}
	else {
		return (current[maxDiffIdx] - start[maxDiffIdx]) / maxDiff;
	}
}

static void GRBLParser_makePosCore_formatNum(std::vector<uint8_t>& cmdInOut, float pos) {
	float posProc = round(pos * 1000) / 1000.0f;
	string valStr = TFormater::float2str(posProc, -1, 3);
	int beforeSize = cmdInOut.size();
	cmdInOut.resize(beforeSize + valStr.length());
	memcpy(&cmdInOut[beforeSize], &valStr[0], valStr.length());
}

// should move to member
const static string EspNowDelayHint = "Cannot sent over NOW";
static bool isEspNowNoSend(vector<uint8_t> dat) {
	if (TFormater::beginWith(dat, EspNowDelayHint)) {
		return true;
	}

	return false;
}

void GRBLParser::sharedCmdProc(std::vector<uint8_t>& cmd) {
}

vector<uint8_t> GRBLParser::noCmd() {
	return vector<uint8_t>();
}

std::vector<uint8_t> GRBLParser::getReplyDelim() {
	std::vector<uint8_t> content;
	content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

vector<uint8_t> GRBLParser::getCmdBuildInfo() {
	std::vector<uint8_t> content;
	content.push_back('$');
	content.push_back('I');
	//content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getCmdReset() {
	std::vector<uint8_t> content;
	content.push_back(0x18);
	content.push_back(0x0a);
	return content;
}

//std::vector<uint8_t> GRBLParser::getReplyResetLegacy() {
//	std::vector<uint8_t> content;
//	content.push_back(0x0d);
//	content.push_back(0x0a);
//
//	string part = "Grbl 0.9j ['$' for help]";
//	insertString(content, part);
//
//	content.push_back(0x0d);
//	content.push_back(0x0a);
//	return content;
//}

std::vector<uint8_t> GRBLParser::getReplyReset() {
	std::vector<uint8_t> content;
	//content.push_back(0x0d);
	//content.push_back(0x0a);

	string part = "Grbl init";
	insertString(content, part);

	content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getCmdStateQuarry() {
	std::vector<uint8_t> content;
	content.push_back('?');
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getCmdConfigQuarry() {
	std::vector<uint8_t> content;
	content.push_back('$');
	content.push_back('$');
	//content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

vector<uint8_t> GRBLParser::getCmdSetConfig(std::string codeStr, std::string valStr) {
	string cmdStr = "$" + codeStr + "=" + valStr;
	std::vector<uint8_t> content(cmdStr.length());
	memcpy(&content[0], cmdStr.c_str(), cmdStr.length());
	//content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getCmdCoordOffsetsQuarry() {
	std::vector<uint8_t> content;
	content.push_back('$');
	content.push_back('#');
	//content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getCmdSetXMaxSpeed(float mmPerMin) {
	std::vector<uint8_t> content;
	insertString(content, paraSetXMaxSpeed);
	insertString(content, TFormater::float2str(mmPerMin, -1, 3));
	// content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getCmdSetYMaxSpeed(float mmPerMin) {
	std::vector<uint8_t> content;
	insertString(content, paraSetYMaxSpeed);
	insertString(content, TFormater::float2str(mmPerMin, -1, 3));
	// content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getCmdSetZMaxSpeed(float mmPerMin) {
	std::vector<uint8_t> content;
	insertString(content, paraSetZMaxSpeed);
	insertString(content, TFormater::float2str(mmPerMin, -1, 3));
	// content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getCmdSetXMaxAcc(float mmPerSecSq) {
	std::vector<uint8_t> content;
	insertString(content, paraSetXMaxAcc);
	insertString(content, TFormater::float2str(mmPerSecSq, -1, 3));
	// content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getCmdSetYMaxAcc(float mmPerSecSq) {
	std::vector<uint8_t> content;
	insertString(content, paraSetYMaxAcc);
	insertString(content, TFormater::float2str(mmPerSecSq, -1, 3));
	// content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getCmdSetZMaxAcc(float mmPerSecSq) {
	std::vector<uint8_t> content;
	insertString(content, paraSetZMaxAcc);
	insertString(content, TFormater::float2str(mmPerSecSq, -1, 3));
	// content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

//std::vector<uint8_t> GRBLParser::getCmdGoToMachinePos(bool flagMovX, float movXDist, bool flagMovY, float movYDist, bool flagMovZ, float movZDist) {
//	std::vector<uint8_t> content;
//	insertString(content, paraAbsPosMode);
//	insertString(content, paraMove);
//
//	vector<float>poses;	poses.push_back(movXDist); poses.push_back(movYDist); poses.push_back(movZDist);
//	int mask = (flagMovX ? 0x01 : 0x00) | (flagMovY ? 0x02 : 0x00) | (flagMovZ ? 0x04 : 0x00);
//
//	makePosCore(content, poses.size(), poses.data(), mask);
//	content.push_back(0x0d);
//	content.push_back(0x0a);
//
//	return content;
//}

std::vector<uint8_t> GRBLParser::getCmdSetWorkCoordTempOrigin() {
	std::vector<uint8_t> content;
	insertString(content, paraSetTempWorkPos);
	insertString(content, "X0Y0Z0A0B0C0");
	content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getCmdSetWorkCoordTempPosition(float x, float y, float z) {
	std::vector<uint8_t> content;
	string val = string("X") + TFormater::float2str(x, -1, 3) + "Y" + TFormater::float2str(y, -1, 3) + "Z" + TFormater::float2str(z, -1, 3);
	insertString(content, paraSetTempWorkPos);
	insertString(content, val);
	content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getCmdRemoveWorkCoordTempOrigin() {
	std::vector<uint8_t> content;
	insertString(content, paraRemoveTempWorkPos);
	content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getCmdGoToWorkPos(
	int axisIdx, float dist) {
	std::vector<uint8_t> content;
	insertString(content, paraAbsPosMode);
	insertString(content, paraMove);

	content.push_back(axisCodes[axisIdx]);
	GRBLParser_makePosCore_formatNum(content, dist);

	content.push_back(0x0d);
	content.push_back(0x0a);

	return content;
}

std::vector<uint8_t> GRBLParser::getCmdGoToWorkPos(bool flagMovX, float movXDist, bool flagMovY, float movYDist, bool flagMovZ, float movZDist) {
	std::vector<uint8_t> content;
	insertString(content, paraAbsPosMode);
	insertString(content, paraMove);

	vector<float>poses;	poses.push_back(movXDist); poses.push_back(movYDist); poses.push_back(movZDist);
	int mask = (flagMovX ? 0x01 : 0x00) | (flagMovY ? 0x02 : 0x00) | (flagMovZ ? 0x04 : 0x00);

	makePosCore(content, poses.size(), poses.data(), mask);
	content.push_back(0x0d);
	content.push_back(0x0a);

	return content;
}

std::vector<uint8_t> GRBLParser::getCmdGoToWorkPos(
	std::vector<float> dists, int mask) {
	std::vector<uint8_t> content;
	insertString(content, paraAbsPosMode);
	insertString(content, paraMove);
	makePosCore(content, dists.size(), dists.data(), mask);
	content.push_back(0x0d);
	content.push_back(0x0a);

	return content;
}

std::vector<uint8_t> GRBLParser::getCmdGoToWorkPosAtRate(
	std::vector<float> dists, int mask,
	float mmPerMin) {
	std::vector<uint8_t> content;
	insertString(content, paraAbsPosMode);
	insertString(content, paraMoveWithSpeed);
	makePosCore(content, dists.size(), dists.data(), mask);

	insertString(content, paraMoveSpeedOverwrite);
	insertString(content, TFormater::float2str(mmPerMin, -1, 0));

	content.push_back(0x0d);
	content.push_back(0x0a);

	return content;
}

//std::vector<uint8_t> GRBLParser::getCmdGoToWorkPos(
//	float movXDist,
//	float movYDist,
//	float movZDist,
//	float movADist,
//	float movBDist,
//	float movCDist, int mask) {
//	std::vector<uint8_t> content;
//	insertString(content, paraAbsPosMode);
//	insertString(content, paraMove);
//	makePosCore(content,
//		movXDist,
//		movYDist,
//		movZDist,
//		movADist,
//		movBDist,
//		movCDist, mask);
//	content.push_back(0x0d);
//	content.push_back(0x0a);
//
//	return content;
//}

std::vector<uint8_t> GRBLParser::getCmdMoveDistance(
	int axisIdx, float dist) {
	std::vector<uint8_t> content;
	insertString(content, paraRltPosMode);
	insertString(content, paraMove);

	content.push_back(axisCodes[axisIdx]);
	GRBLParser_makePosCore_formatNum(content, dist);

	content.push_back(0x0d);
	content.push_back(0x0a);

	return content;
}

std::vector<uint8_t> GRBLParser::getCmdMoveDistance(bool flagMovX, float movXDist, bool flagMovY, float movYDist, bool flagMovZ, float movZDist) {
	std::vector<uint8_t> content;
	insertString(content, paraRltPosMode);
	insertString(content, paraMove);

	vector<float>poses;	poses.push_back(movXDist); poses.push_back(movYDist); poses.push_back(movZDist);
	int mask = (flagMovX ? 0x01 : 0x00) | (flagMovY ? 0x02 : 0x00) | (flagMovZ ? 0x04 : 0x00);

	makePosCore(content, poses.size(), poses.data(), mask);
	content.push_back(0x0d);
	content.push_back(0x0a);

	return content;
}

std::vector<uint8_t> GRBLParser::getCmdMoveDistance(
	std::vector<float> dists, int mask) {
	std::vector<uint8_t> content;
	insertString(content, paraRltPosMode);
	insertString(content, paraMove);
	makePosCore(content, dists.size(), dists.data(), mask);
	content.push_back(0x0d);
	content.push_back(0x0a);

	return content;
}
//
//std::vector<uint8_t> GRBLParser::getCmdGoToRltPos(
//	float movXDist,
//	float movYDist,
//	float movZDist,
//	float movADist,
//	float movBDist,
//	float movCDist, int mask) {
//	std::vector<uint8_t> content;
//	insertString(content, paraRltPosMode);
//	insertString(content, paraMove);
//	makePosCore(content,
//		movXDist,
//		movYDist,
//		movZDist,
//		movADist,
//		movBDist,
//		movCDist, mask);
//	content.push_back(0x0d);
//	content.push_back(0x0a);
//
//	return content;
//}

//std::vector<uint8_t> GRBLParser::getCmdGoToRltPosAtRate(
//	float movXDist,
//	float movYDist,
//	float movZDist,
//	float movADist,
//	float movBDist,
//	float movCDist, int mask,
//	float mmPerMin) {
//	std::vector<uint8_t> content;
//	insertString(content, paraRltPosMode);
//	insertString(content, paraMoveWithSpeed);
//	makePosCore(content,
//		movXDist,
//		movYDist,
//		movZDist,
//		movADist,
//		movBDist,
//		movCDist, mask);
//
//	insertString(content, paraMoveSpeedOverwrite);
//	insertString(content, TFormater::float2str(mmPerMin, -1, 0));
//
//	content.push_back(0x0d);
//	content.push_back(0x0a);
//
//	return content;
//}

std::vector<uint8_t> GRBLParser::getCmdMoveDistanceAtRate(
	int axisIdx, int mask, float mmPerMin) {
	std::vector<uint8_t> content;
	insertString(content, paraRltPosMode);
	insertString(content, paraMoveWithSpeed);
	makePosCore(content, axisIdx, mask);

	insertString(content, paraMoveSpeedOverwrite);
	insertString(content, TFormater::float2str(mmPerMin, -1, 0));

	content.push_back(0x0d);
	content.push_back(0x0a);

	return content;
}

std::vector<uint8_t> GRBLParser::getCmdMoveDistanceAtRate(
	std::vector<float> dists, int mask, float mmPerMin) {
	std::vector<uint8_t> content;
	insertString(content, paraRltPosMode);
	insertString(content, paraMoveWithSpeed);
	makePosCore(content,
		dists.size(), dists.data(), mask);

	insertString(content, paraMoveSpeedOverwrite);
	insertString(content, TFormater::float2str(mmPerMin, -1, 3));

	content.push_back(0x0d);
	content.push_back(0x0a);

	return content;
}

vector<uint8_t> GRBLParser::getGeneralReply() {
	std::vector<uint8_t> content;
	insertString(content, stateCmdReply);
	content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getConfigReply() {
	std::vector<uint8_t> content;
	insertString(content, stateCmdReply);
	content.push_back(0x0d);
	content.push_back(0x0a);
	//insertString(content, stateCmdReply);
	//content.push_back(0x0d);
	//content.push_back(0x0a);
	return content;
}

vector<uint8_t> GRBLParser::getCmdStop() {
	return getCmdReset();
}

std::vector<uint8_t> GRBLParser::getReplyStop() {
	return getReplyReset();
}

vector<uint8_t> GRBLParser::getCmdUnlock() {
	std::vector<uint8_t> content;
	content.push_back('$');
	content.push_back('X');
	//content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

// message disabled as    #define GRBL_MSG_LEVEL MsgLevel::Error in Config.h
vector<uint8_t> GRBLParser::getReplyUnlocked() {
	std::vector<uint8_t> content;
	insertString(content, stateCmdReply);
	content.push_back(0x0d);
	content.push_back(0x0a);
	//insertString(content, stateCmdReply);
	//content.push_back(0x0d);
	//content.push_back(0x0a);

	return content;
}

vector<uint8_t> GRBLParser::getNoteToUnLock() {
	std::vector<uint8_t> content;
	insertString(content, keyToUnlock);
	content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getReplyHardLimitedIdle() {
	std::vector<uint8_t> content;
	insertString(content, keyHardLimitedIdle);
	content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getReplyHardLimitedRun() {
	std::vector<uint8_t> content;
	insertString(content, keyHardLimitedRun);
	content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

//std::vector<uint8_t> GRBLParser::getReplyHardLimited1() {
//	std::vector<uint8_t> content;
//	insertString(content, keyHardLimited1);
//	content.push_back(0x0d);
//	content.push_back(0x0a);
//	return content;
//}

// when disabled, change takes no effect until reboot
std::vector<uint8_t> GRBLParser::getCmdEnableHardLimit() {
	std::vector<uint8_t> content;
	insertString(content, paraEnableHardLimit);
	//content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

// when disabled, change takes no effect until reboot
std::vector<uint8_t> GRBLParser::getCmdDisableHardLimit() {
	std::vector<uint8_t> content;
	insertString(content, paraDisableHardLimit);
	//content.push_back(0x0d);
	content.push_back(0x0a);
	return content;
}

vector<uint8_t> GRBLParser::getCmdEnableHoming(bool flagEnable) {
	std::vector<uint8_t> content;
	content.push_back('$');
	content.push_back('2');
	content.push_back('2');
	content.push_back('=');
	if (flagEnable) {
		content.push_back('1');
	}
	else {
		content.push_back('0');
	}
	content.push_back(0x0a);
	return content;
}

// see Homing direction in https://github.com/gnea/grbl/wiki/Set-up-the-Homing-Cycle
vector<uint8_t> GRBLParser::getCmdSetHomingDirection(int xDir, int yDir, int zDir) {
	std::vector<uint8_t> content;
	content.push_back('$');
	content.push_back('2');
	content.push_back('3');
	content.push_back('=');

	xDir = (xDir == 1) ? 0 : 1; // 1
	yDir = (yDir == 1) ? 0 : 1; // -1
	zDir = (zDir == 1) ? 0 : 1; // -1

	int val = (zDir << 2) + (yDir << 1) + xDir;
	content.push_back(TFormater::int2str(val)[0]);
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getCmdSetHomingFeedSpeed(int feedMMPerMin) {
	std::vector<uint8_t> content;
	content.push_back('$');
	content.push_back('2');
	content.push_back('4');
	content.push_back('=');
	insertString(content, TFormater::int2str(feedMMPerMin));
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getCmdSetHomingSeekSpeed(int seekMMPerMin) {
	std::vector<uint8_t> content;
	content.push_back('$');
	content.push_back('2');
	content.push_back('5');
	content.push_back('=');
	insertString(content, TFormater::int2str(seekMMPerMin));
	content.push_back(0x0a);
	return content;
}

vector<uint8_t> GRBLParser::getCmdStartHoming(int axis, uint32_t fullBitMask) {
	std::vector<uint8_t> content;
	content.push_back('$');
	content.push_back('H');
	if ((axis & fullBitMask) == 0) {
		// home all
	}
	else {
		if ((axis & 0b000001) == 0b000001) {
			content.push_back('X');
		}
		if ((axis & 0b000010) == 0b000010) {
			content.push_back('Y');
		}
		if ((axis & 0b000100) == 0b000100) {
			content.push_back('Z');
		}
		if ((axis & 0b001000) == 0b001000) {
			content.push_back('A');
		}
		if ((axis & 0b010000) == 0b010000) {
			content.push_back('B');
		}
		if ((axis & 0b100000) == 0b100000) {
			content.push_back('C');
		}
	}
	content.push_back(0x0a);
	return content;
}

// G54 - G59 work coordinates can be changed via the G10 L2 Px command
vector<uint8_t> GRBLParser::getCmdSetCoord(int coordIdx, float x, float y) {
	std::vector<uint8_t> content;
	string cmdStr = paraSetOffset + TFormater::int2str(coordIdx + 1)
		+ "X" + TFormater::float2str(x, -1, 3)
		+ "Y" + TFormater::float2str(y, -1, 3);
	insertString(content, cmdStr);
	content.push_back(0x0a);
	return content;
}

vector<uint8_t> GRBLParser::getCmdSetCoord(int coordIdx, float x, float y, float z, float a, float b, float c, int mask) {
	std::vector<uint8_t> content;
	string cmdStr = paraSetOffset + TFormater::int2str(coordIdx + 1)
		+ "X" + TFormater::float2str(x, -1, 3)
		+ "Y" + TFormater::float2str(y, -1, 3)
		+ "Z" + TFormater::float2str(z, -1, 3)
		+ "A" + TFormater::float2str(a, -1, 3)
		+ "B" + TFormater::float2str(b, -1, 3)
		+ "C" + TFormater::float2str(c, -1, 3);
	insertString(content, cmdStr);
	content.push_back(0x0a);
	return content;
}

vector<uint8_t> GRBLParser::getCmdSwitchToCoord(int coordIdx) {
	std::vector<uint8_t> content;
	string coordName = GRBLParser::translateCoordIndex2Gxx(coordIdx);
	insertString(content, coordName);
	content.push_back(0x0a);
	return content;
}

std::vector<uint8_t> GRBLParser::getCmdPrintParameters(int coordIdx) {
	std::vector<uint8_t> content;
	content.push_back('$');
	content.push_back('$');
	content.push_back(0x0a);
	return content;
}

bool GRBLParser::parseIsIdle(std::vector<uint8_t> replyIn) {
	string replyStr = cmd2str(replyIn);
	bool idleConfirm = replyStr.find(stateIdle) != replyStr.npos;
	return idleConfirm;
}

bool GRBLParser::parseIsMoving(std::vector<uint8_t> replyIn) {
	string replyStr = cmd2str(replyIn);
	bool moveConfirm = replyStr.find(stateMove) != replyStr.npos;
	return moveConfirm;
}

bool GRBLParser::parseIsAlarm(std::vector<uint8_t> replyIn) {
	string replyStr = cmd2str(replyIn);
	bool alarmConfirm = replyStr.find(stateAlarm) != replyStr.npos;
	return alarmConfirm;
}

bool GRBLParser::parseIsHoming(std::vector<uint8_t> replyIn) {
	string replyStr = cmd2str(replyIn);
	bool alarmConfirm = replyStr.find(stateHoming) != replyStr.npos;
	return alarmConfirm;
}

std::vector<float> GRBLParser::parseMachinePos(std::vector<uint8_t> replyIn) {
	string replyStr = cmd2str(replyIn);
	size_t pos = replyStr.find(keyMachinePos);
	if (pos == string::npos) {
		return vector<float>();
	}
	else {
		return parsePosCore(replyIn, pos + keyMachinePos.length());
	}
}

std::vector<float> GRBLParser::parseWorkPos(std::vector<uint8_t> replyIn) {
	string replyStr = cmd2str(replyIn);
	size_t pos = replyStr.find(keyWorkPos);
	if (pos == string::npos) {
		return vector<float>();
	}
	else {
		return parsePosCore(replyIn, pos + keyWorkPos.length());
	}
}

std::vector<int> GRBLParser::parseMissingAxises(std::vector<uint8_t> replyIn) {
	string replyStr = cmd2str(replyIn);
	size_t pos = replyStr.find(keyWorkPos);
	if (pos == string::npos) {
		return vector<int>();
	}
	else {
		return parseMissingAxisCore(replyIn, pos + keyWorkPos.length());
	}
}

std::vector<int> GRBLParser::parseHardLimit(std::vector<uint8_t> replyIn) {
	std::vector<int> ret(3, 0);
	TFormater::StrLikeU8 content(replyIn);
	vector<uint8_t> startPhrase;
	startPhrase.push_back('L');
	startPhrase.push_back('i');
	startPhrase.push_back('m');
	startPhrase.push_back(':');
	size_t offset = content.find(startPhrase);
	if (offset == std::string::npos) {
		return std::vector<int>();
	}
	TFormater::StrLikeU8 posStr = content.substr(offset + startPhrase.size(), 3);
	for (uint8_t val : posStr.content) {
		if (val == 'x') {
			ret[0] = -1;
		}
		else if (val == 'X') {
			ret[0] = 1;
		}
		else if (val == 'y') {
			ret[1] = -1;
		}
		else if (val == 'Y') {
			ret[1] = 1;
		}
		else if (val == 'z') {
			ret[2] = -1;
		}
		else if (val == 'Z') {
			ret[2] = 1;
		}
	}
	return ret;
}

int GRBLParser::parseHardLimitInt(std::vector<uint8_t> replyIn) {
	TFormater::StrLikeU8 content(replyIn);
	vector<uint8_t> startPhrase;
	startPhrase.push_back('L');
	startPhrase.push_back('i');
	startPhrase.push_back('m');
	startPhrase.push_back(':');
	size_t offset = content.find(startPhrase);
	if (offset == std::string::npos) {
		return 0;
	}
	TFormater::StrLikeU8 posStr = content.substr(offset + startPhrase.size(), 3);
	int ret = 0;
	for (uint8_t val : posStr.content) {
		if (val == 'x') {
			ret |= 0x0001;
		}
		else if (val == 'X') {
			ret |= 0x0002;
		}
		else if (val == 'y') {
			ret |= 0x0004;
		}
		else if (val == 'Y') {
			ret |= 0x0008;
		}
		else if (val == 'z') {
			ret |= 0x0010;
		}
		else if (val == 'Z') {
			ret |= 0x0020;
		}
		else if (val == 'a') {
			ret |= 0x0040;
		}
		else if (val == 'A') {
			ret |= 0x0080;
		}
		else if (val == 'b') {
			ret |= 0x0100;
		}
		else if (val == 'B') {
			ret |= 0x0200;
		}
		else if (val == 'c') {
			ret |= 0x0400;
		}
		else if (val == 'C') {
			ret |= 0x0800;
		}
	}
	return ret;
}

std::string GRBLParser::parseSensors(std::vector<uint8_t> replyIn) {
	std::string ret;

	TFormater::StrLikeU8 content(replyIn);
	vector<uint8_t> startPhrase;
	startPhrase.push_back('S');
	startPhrase.push_back('e');
	startPhrase.push_back('n');
	startPhrase.push_back(':');
	size_t offset = content.find(startPhrase);
	if (offset == std::string::npos) {
		return "";
	}
	offset = offset + startPhrase.size();
	for (; offset < replyIn.size(); offset++) {
		if (replyIn[offset] != '>') {
			ret.push_back(replyIn[offset]);
		}
		else {
			ret.push_back('\0');
		}
	}
	return ret;
}

std::string GRBLParser::toDisplayable(std::vector<uint8_t>& content, char delim) {
	if (content.size() == 0) {
		return "";
	}
	return TFormater::toDisplayable(&content[0], content.size(), delim);
}

std::string GRBLParser::translateCoordIndex2Gxx(int coordIdx) {
	string coordStr = "G" + TFormater::int2str(54 + coordIdx);
	return coordStr;
}

void GRBLParser::insertString(std::vector<uint8_t>& cmdInOut, std::string cmd) {
	TFormater::str2u8(cmd, cmdInOut);
}

// multi channel?
//void GRBLParser::makePosCore(std::vector<uint8_t>& cmdInOut,
//	bool flagMovX, float movXDist,
//	bool flagMovY, float movYDist,
//	bool flagMovZ, float movZDist) {
//	if (flagMovX) {
//		cmdInOut.push_back('X');
//		GRBLParser_makePosCore_formatNum(cmdInOut, movXDist);
//	}
//	if (flagMovY) {
//		cmdInOut.push_back('Y');
//		GRBLParser_makePosCore_formatNum(cmdInOut, movYDist);
//	}
//	if (flagMovZ) {
//		cmdInOut.push_back('Z');
//		GRBLParser_makePosCore_formatNum(cmdInOut, movZDist);
//	}
//}
//
//void GRBLParser::makePosCore(std::vector<uint8_t>& cmdInOut,
//	float movXDist,
//	float movYDist,
//	float movZDist,
//	float movADist,
//	float movBDist,
//	float movCDist, int mask) {
//	if (X_MASKED(mask)) {
//		cmdInOut.push_back('X');
//		GRBLParser_makePosCore_formatNum(cmdInOut, movXDist);
//	}
//	if (Y_MASKED(mask)) {
//		cmdInOut.push_back('Y');
//		GRBLParser_makePosCore_formatNum(cmdInOut, movYDist);
//	}
//	if (Z_MASKED(mask)) {
//		cmdInOut.push_back('Z');
//		GRBLParser_makePosCore_formatNum(cmdInOut, movYDist);
//	}
//	if (A_MASKED(mask)) {
//		cmdInOut.push_back('A');
//		GRBLParser_makePosCore_formatNum(cmdInOut, movADist);
//	}
//	if (B_MASKED(mask)) {
//		cmdInOut.push_back('B');
//		GRBLParser_makePosCore_formatNum(cmdInOut, movBDist);
//	}
//	if (C_MASKED(mask)) {
//		cmdInOut.push_back('C');
//		GRBLParser_makePosCore_formatNum(cmdInOut, movCDist);
//	}
//}

void GRBLParser::makePosCore(std::vector<uint8_t>& cmdInOut,
	int axisIdx, float position) {
	if (axisIdx >= 0 && axisIdx < axisCodes.size()) {
		cmdInOut.push_back(axisCodes[axisIdx]);
		GRBLParser_makePosCore_formatNum(cmdInOut, position);
	}
}

void GRBLParser::makePosCore(std::vector<uint8_t>& cmdInOut,
	int axisNum, float* movXYZABC,
	int mask) {
	for (int idx = 0; idx < min(axisNum, axisCodes.size()); idx++) {
		if (AXIS_MASKED(mask, idx)) {
			cmdInOut.push_back(axisCodes[idx]);
			GRBLParser_makePosCore_formatNum(cmdInOut, movXYZABC[idx]);
		}
	}
	//if ((axisNum >= 1) && X_MASKED(mask)) {
	//	cmdInOut.push_back(axisCodes[0]);
	//	GRBLParser_makePosCore_formatNum(cmdInOut, movXYZABC[0]);
	//}
	//if ((axisNum >= 2) && Y_MASKED(mask)) {
	//	cmdInOut.push_back('Y');
	//	GRBLParser_makePosCore_formatNum(cmdInOut, movXYZABC[1]);
	//}
	//if ((axisNum >= 3) && Z_MASKED(mask)) {
	//	cmdInOut.push_back('Z');
	//	GRBLParser_makePosCore_formatNum(cmdInOut, movXYZABC[2]);
	//}
	//if ((axisNum >= 4) && A_MASKED(mask)) {
	//	cmdInOut.push_back('A');
	//	GRBLParser_makePosCore_formatNum(cmdInOut, movXYZABC[3]);
	//}
	//if ((axisNum >= 5) && B_MASKED(mask)) {
	//	cmdInOut.push_back('B');
	//	GRBLParser_makePosCore_formatNum(cmdInOut, movXYZABC[4]);
	//}
	//if ((axisNum >= 6) && C_MASKED(mask)) {
	//	cmdInOut.push_back('C');
	//	GRBLParser_makePosCore_formatNum(cmdInOut, movXYZABC[5]);
	//}
}

std::vector<float> GRBLParser::parsePosCore(std::vector<uint8_t>& cmdInOut, size_t offset) {
	std::vector<float> pos;
	size_t startPos = offset;
	while (startPos < cmdInOut.size() && pos.size() <= axisCodes.size()) {
		for (size_t endPos = startPos + 1; endPos < cmdInOut.size(); endPos++) {
			if (cmdInOut[endPos] >= 'a' && cmdInOut[endPos] <= 'z') {
				return pos;
			}
			if (cmdInOut[endPos] >= 'A' && cmdInOut[endPos] <= 'Z') {
				return pos;
			}
			if (cmdInOut[endPos] != '.' && (cmdInOut[endPos] < '0' || cmdInOut[endPos]>'9')) {
				char* valChars = new char[endPos - startPos + 1];
				memcpy(valChars, &cmdInOut[startPos], endPos - startPos);
				valChars[endPos - startPos] = '\0';
				if (valChars[0] == '?') {
					pos.push_back(0);
				}
				else {
					float val = TFormater::str2float(valChars);
					pos.push_back(val);
				}
				startPos = endPos + 1;
				delete[] valChars;
				break;
			}
		}
	}
	return pos;
}

std::vector<int> GRBLParser::parseMissingAxisCore(std::vector<uint8_t>& cmdInOut, size_t offset) {
	std::vector<int> idxes;
	int axIdx = 0;
	for (size_t cPos = offset; cPos < cmdInOut.size(); cPos++) {
		if (cPos == '?') {
			idxes.push_back(axIdx);
		}
		else if (cmdInOut[cPos] == ',') {
			axIdx++;
		}
		else if (cmdInOut[cPos] != '.' && (cmdInOut[cPos] < '0' || cmdInOut[cPos]>'9')) {
			// part of number
		}
		else {
			return idxes;
		}
	}
}

std::string GRBLParser::cmd2str(std::vector<uint8_t> content) {
	content.push_back('\0');
	string contentStr = string((char*)(&content[0]));
	return contentStr;
}

//PlatformLogCallback  _core.sentCB = NULL;
//PlatformLogCallback GRBLMaster::replyCB = NULL;
//PlatformLogCallback GRBLMaster::timeoutCB = NULL;

//GRBLMaster::PlatformConfig GRBLMaster::_platformSlaveConfig;
//int GRBLMaster::platformCoordSystemIdx = -1;

const int GRBLMaster::platformResetReplyTimeOut = 4000;
const int GRBLMaster::platformNoteToResetTimeout = 1000;
const int GRBLMaster::platformGeneralCmdReplyTimeOut = 200;
const int GRBLMaster::platformConfigReplyTimeOut = 1000;
const int GRBLMaster::stateQuarryReplyTimeOut = 500;
const int GRBLMaster::goToCmdReplyTimeOut = 100;
const int GRBLMaster::stopTimeOut = 200;

GRBLMaster::GRBLMaster() {
	int stop = 1;
}

GRBLMaster::~GRBLMaster() {
	int stop = 1;
}

void GRBLMaster::init() {
	_state.homed();
}

void GRBLMaster::purge() {
	if (_core.slaveNativeHandle != nullptr) {
		//writeLine("Purge pfSlave");
		SlaveComm->purge();
	}
}

void GRBLMaster::setReceiveAttemptAllowance(int number) {
	_flags.receiveAttemptAllowance = number;
}

bool GRBLMaster::isSimulating() {
	return _flags.simuMode != 0;
}

bool GRBLMaster::canControlPlatform() {
	return (_core.slaveNativeHandle != nullptr) && (SlaveComm->isConnected());
}

//bool GRBLMaster::checkPlatformHardLimitedLegacy() {
//	vector<uint8_t> stateReply = _core.peekPlatformRply();
//	if (stateReply.size() == 0) {
//		return false;
//	}
//	return true;
	//vector<uint8_t> hardLimitAlarm0 = GRBLParser::getReplyHardLimited0();

	//int offset = SerialPortManager::compareBuffers(&stateReply[0], stateReply.size(), &hardLimitAlarm0[0], hardLimitAlarm0.size());
	//if (offset >= 0) {
	//	return true;
	//}
	//else {
	//}
	//return false;
//}

//ALARM: Hard limit
//[Reset to continue]
//
//Grbl 0.9j['$' for help]
//['$H' | '$X' to unlock]
//
//[Caution:Unlocked]


// 1.3a
// ALARM:1

//int GRBLMaster::unlockFromLimitLegacy() {
	//if ( _core.receiveRplyCore(GRBLParser::getReplyHardLimited0(), platformNoteToResetTimeout).size() > 0) {
	//	if ( _core.receiveRplyCore(GRBLParser::getReplyHardLimited1(), platformNoteToResetTimeout).size() > 0) {

	//		if ( _core.sendCmdCheckReply(GRBLParser::getCmdReset(),
	//			platformResetReplyTimeOut, GRBLParser::getReplyResetLegacy())) {
	//			if ( _core.receiveRplyCore(GRBLParser::getNoteToUnLock(), platformResetReplyTimeOut).size() > 0) {

	//				return  _core.sendCmdCheckReply(GRBLParser::getCmdUnlock(),
	//					platformResetReplyTimeOut, GRBLParser::getReplyUnlocked(platformLagacyVer == 0));

	//			}
	//		}

	//	}
	//}
	//return 0;
//	return 0;
//}

bool GRBLMaster::softRebootPlatform() {
	if (_flags.simuMode > 0) {
		return true;
	}
	purge();
	//if (platformLagacyVer == 1) {
	//	return Core::sendCmdCheckReply(GRBLParser::getCmdReset(),
	//		platformResetReplyTimeOut, GRBLParser::getReplyResetLegacy());
	//}
	//else {
	return  _core.sendCmdCheckReply(GRBLParser::getCmdReset(),
		platformResetReplyTimeOut, GRBLParser::getReplyReset());
	//}
}

bool GRBLMaster::hardRebootPlatform() {
	if (_flags.simuMode > 0) {
		return true;
	}
	if (_core.slaveNativeHandle != nullptr) {
		TLogger logger("Hard reboot platform");
		SlaveComm->purge();
		SlaveComm->setRts(1);
		SlaveComm->setDtr(1);
		Sleep(500);
		SlaveComm->setDtr(0);
		Sleep(500);
		SlaveComm->setDtr(1);

		std::vector<uint8_t> expReply = GRBLParser::getReplyReset();
		std::vector<uint8_t> reply = _core.receiveRplyCore(expReply, platformResetReplyTimeOut);
		vector<int> found = SerialPortManager::find(&reply[0], reply.size(), &expReply[0], expReply.size());
		SlaveComm->purge();
		TLogger::writeLine("Complete");
		return found.size() > 0;
	}
	else {
		return false;
	}
}

bool GRBLMaster::unlockPlatform() {
	if (_flags.simuMode > 0) {
		return true;
	}
	return  _core.sendCmdCheckReply(GRBLParser::getCmdUnlock(),
		platformGeneralCmdReplyTimeOut, GRBLParser::getReplyUnlocked());
}

//bool resetPlatformFromLock() {
//	purge(true, false);
//	if ( _core.sendCmdCheckReply(GRBLParser::getCmdReset(),
//		platformResetReplyTimeOut, GRBLParser::getReplyReset())) {
//		return  _core.sendCmdCheckReply(GRBLParser::getCmdUnlock(),
//			platformGeneralCmdReplyTimeOut, GRBLParser::getReplyUnlocked(platformLagacyVer == 0));
//	}
//	return false;
//}

bool GRBLMaster::quickPlatformTest() {
	if (_flags.simuMode > 0) {
		return true;
	}
	return getPlatformState(nullptr, 0, nullptr, nullptr);
}

bool GRBLMaster::setFlagHardLimit(bool flagIn) {
	if (_flags.simuMode > 0) {
		return true;
	}
	bool ret;
	if (flagIn) {
		ret = _core.sendCmdCheckReply(GRBLParser::getCmdEnableHardLimit(),
			platformResetReplyTimeOut, GRBLParser::getConfigReply());
	}
	else {
		ret = _core.sendCmdCheckReply(GRBLParser::getCmdDisableHardLimit(),
			platformResetReplyTimeOut, GRBLParser::getConfigReply());
	}

	ret &= updateConfigFromSlave();

	return ret;
}

bool GRBLMaster::updateConfigFromSlave() {
	if (_flags.simuMode > 0) {
		return true;
	}
	if (_core.sendCmdCore(GRBLParser::getCmdConfigQuarry())) {
		//std::vector<unsigned char> okStr =  _core.receiveRplyCore(GRBLParser::getGeneralReply(), platformGeneralCmdReplyTimeOut);
		std::vector<unsigned char> configStr = _core.receiveRplyCore(GRBLParser::getConfigReply(), platformConfigReplyTimeOut, EspNowDelayHint);
		if (configStr.size() == 0)
		{
			return false;
		}
		if (!TFormater::beginWith(configStr, "$")) {
			return false;
		}
		_platformSlaveConfig.setConfig(false, configStr);
		return true;
	}
	return false;
}

bool GRBLMaster::setXMaxSpeed(float mmPerMin) {
	if (_flags.simuMode > 0) {
		return true;
	}
	vector<uint8_t> cmd = GRBLParser::getCmdSetXMaxSpeed(mmPerMin);
	if (_platformSlaveConfig.matchConfigNSetIfNot(cmd)) {
		return true;
	}
	vector<uint8_t> expReply = GRBLParser::getConfigReply();

	return  _core.sendCmdCheckReply(cmd, platformGeneralCmdReplyTimeOut, expReply);
}

bool GRBLMaster::setYMaxSpeed(float mmPerMin) {
	if (_flags.simuMode > 0) {
		return true;
	}
	vector<uint8_t> cmd = GRBLParser::getCmdSetYMaxSpeed(mmPerMin);
	if (_platformSlaveConfig.matchConfigNSetIfNot(cmd)) {
		return true;
	}
	vector<uint8_t> expReply = GRBLParser::getConfigReply();

	return  _core.sendCmdCheckReply(cmd, platformGeneralCmdReplyTimeOut, expReply);
}

bool GRBLMaster::setZMaxSpeed(float mmPerMin) {
	if (_flags.simuMode > 0) {
		return true;
	}
	vector<uint8_t> cmd = GRBLParser::getCmdSetZMaxSpeed(mmPerMin);
	if (_platformSlaveConfig.matchConfigNSetIfNot(cmd)) {
		return true;
	}
	vector<uint8_t> expReply = GRBLParser::getConfigReply();

	return  _core.sendCmdCheckReply(cmd, platformGeneralCmdReplyTimeOut, expReply);
}

bool GRBLMaster::setXMaxAcc(float mmPerMin) {
	if (_flags.simuMode > 0) {
		return true;
	}
	vector<uint8_t> cmd = GRBLParser::getCmdSetXMaxAcc(mmPerMin);
	if (_platformSlaveConfig.matchConfigNSetIfNot(cmd)) {
		return true;
	}
	vector<uint8_t> expReply = GRBLParser::getConfigReply();

	return  _core.sendCmdCheckReply(cmd, platformGeneralCmdReplyTimeOut, expReply);
}

bool GRBLMaster::setYMaxAcc(float mmPerMin) {
	if (_flags.simuMode > 0) {
		return true;
	}
	vector<uint8_t> cmd = GRBLParser::getCmdSetYMaxAcc(mmPerMin);
	if (_platformSlaveConfig.matchConfigNSetIfNot(cmd)) {
		return true;
	}
	vector<uint8_t> expReply = GRBLParser::getConfigReply();

	return  _core.sendCmdCheckReply(cmd, platformGeneralCmdReplyTimeOut, expReply);
}

bool GRBLMaster::setZMaxAcc(float mmPerMin) {
	if (_flags.simuMode > 0) {
		return true;
	}
	vector<uint8_t> cmd = GRBLParser::getCmdSetZMaxAcc(mmPerMin);
	if (_platformSlaveConfig.matchConfigNSetIfNot(cmd)) {
		return true;
	}
	vector<uint8_t> expReply = GRBLParser::getConfigReply();

	return  _core.sendCmdCheckReply(cmd, platformGeneralCmdReplyTimeOut, expReply);
}

//bool GRBLMaster::waitUntilNotBuzy(int timeout) {
//	StopWatch sw("\n", true, true);
//	while (true) {
//		if (getPlatformStateQuarry()) {
//			CtrlMovState state;
//			bool ret = getPlatformStateReceive(nullptr, &state,
//				nullptr, nullptr, nullptr, nullptr,
//				nullptr, nullptr, nullptr, nullptr,
//				nullptr, nullptr, nullptr);
//			if (ret) {
//				return true;
//			}
//			if (sw.shoudFire(timeout)) {
//				return false;
//			}
//		}
//		//SlaveComm->receiveBytes();
//		writeLine("waitUntilNotBuzy");
//	}
//}

bool GRBLMaster::setWorkPositionTempOrigin() {
	if (isSimulating()) {
		for (int idx = 0; idx < _state.MaxAxNum; idx++) {
			_state.workPosXYZABC[idx] = 0;
		}
	}
	vector<uint8_t> cmd = GRBLParser::getCmdSetWorkCoordTempOrigin();
	vector<uint8_t> expReply = GRBLParser::getGeneralReply();

	return  _core.sendCmdCheckReply(cmd, platformGeneralCmdReplyTimeOut, expReply);
}

bool GRBLMaster::setWorkPositionTempPosition(float x, float y, float z) {
	vector<uint8_t> cmd = GRBLParser::getCmdSetWorkCoordTempPosition(x, y, z);
	vector<uint8_t> expReply = GRBLParser::getGeneralReply();

	return  _core.sendCmdCheckReply(cmd, platformGeneralCmdReplyTimeOut, expReply);
}

bool GRBLMaster::removeWorkPositionTempOrigin() {
	vector<uint8_t> cmd = GRBLParser::getCmdRemoveWorkCoordTempOrigin();
	vector<uint8_t> expReply = GRBLParser::getGeneralReply();

	return  _core.sendCmdCheckReply(cmd, platformGeneralCmdReplyTimeOut, expReply);
}
//
//bool GRBLMaster::mov2WorkPosAbsMasked(float* xyz, int mask, float rateMMPerMin) {
//	vector<uint8_t> cmd;
//	if (rateMMPerMin > 0) {
//		cmd = GRBLParser::getCmdGoToWorkPosAtRate(
//			xyz, mask, rateMMPerMin);
//	}
//	else {
//		cmd = GRBLParser::getCmdGoToWorkPos(
//			xyz, mask);
//	}
//	if (_flags.simuMode > 0) {
//		if (X_MASKED(mask)) {
//			_state.targetXYZABC[0] = xyz[0];
//		}
//		if (Y_MASKED(mask)) {
//			_state.targetXYZABC[1] = xyz[1];
//		}
//		if (Z_MASKED(mask)) {
//			_state.targetXYZABC[2] = xyz[2];
//		}
//		if (A_MASKED(mask)) {
//			_state.targetXYZABC[3] = xyz[3];
//		}
//		if (B_MASKED(mask)) {
//			_state.targetXYZABC[4] = xyz[4];
//		}
//		if (C_MASKED(mask)) {
//			_state.targetXYZABC[5] = xyz[5];
//		}
//		_state.action = 'R';
//		_core.simuSend(cmd);
//		return true;
//	}
//	return  _core.sendMovCheckReply(cmd);
//}

bool GRBLMaster::mov2WorkPosAbs(float x, float y) {
	vector<uint8_t> cmd = GRBLParser::getCmdGoToWorkPos(
		true, x,
		true, y,
		false, 0);
	return  _core.sendMovCheckReply(cmd);
}

bool GRBLMaster::mov2WorkPosAbs(float x, float y, float z) {
	vector<uint8_t> cmd = GRBLParser::getCmdGoToWorkPos(
		true, x,
		true, y,
		true, z);
	return  _core.sendMovCheckReply(cmd);
}

//bool GRBLMaster::movByDistMasked(float* xyzabc, int mask) {
//	return movAxisesByDist(mask, xyzabc);
	//vector<uint8_t> cmd = GRBLParser::getCmdGoToRltPos(
	//	xyzabc, mask);
	//if (_flags.simuMode > 0) {
	//	if (X_MASKED(mask)) {
	//		_state.targetXYZABC[0] += xyzabc[0];
	//	}
	//	if (Y_MASKED(mask)) {
	//		_state.targetXYZABC[1] += xyzabc[1];
	//	}
	//	if (Z_MASKED(mask)) {
	//		_state.targetXYZABC[2] += xyzabc[2];
	//	}
	//	if (A_MASKED(mask)) {
	//		_state.targetXYZABC[3] += xyzabc[3];
	//	}
	//	if (B_MASKED(mask)) {
	//		_state.targetXYZABC[4] += xyzabc[4];
	//	}
	//	if (C_MASKED(mask)) {
	//		_state.targetXYZABC[5] += xyzabc[5];
	//	}
	//	_state.action = 'R';
	//	_core.simuSend(cmd);
	//	return true;
	//}
	//return  _core.sendMovCheckReply(cmd);
//}

bool GRBLMaster::movHrz(float mmWithSign) {
	if (_flags.simuMode > 0) {
		_state.targetXYZABC[0] += mmWithSign;
		_state.action = 'R';
		return true;
	}
	vector<uint8_t> cmd = GRBLParser::getCmdMoveDistance(true, mmWithSign, false, 0, false, 0);

	return  _core.sendMovCheckReply(cmd);
}

bool GRBLMaster::movVtc(float mmWithSign) {
	if (_flags.simuMode > 0) {
		_state.targetXYZABC[1] += mmWithSign;
		_state.action = 'R';
		return true;
	}
	vector<uint8_t> cmd = GRBLParser::getCmdMoveDistance(false, 0, true, mmWithSign, false, 0);

	return  _core.sendMovCheckReply(cmd);
}
//
//bool GRBLMaster::movPlatformA(float cmdMM) {
//	if (_flags.simuMode > 0) {
//		_state.targetXYZABC[3] += cmdMM;
//		_state.action = 'R';
//		return true;
//	}
//	return false;
//}
//
//bool GRBLMaster::movPlatformB(float cmdMM) {
//	if (_flags.simuMode > 0) {
//		_state.targetXYZABC[4] += cmdMM;
//		_state.action = 'R';
//		return true;
//	}
//	return false;
//}
//
//bool GRBLMaster::movPlatformC(float cmdMM) {
//	if (_flags.simuMode > 0) {
//		_state.targetXYZABC[5] += cmdMM;
//		_state.action = 'R';
//		return true;
//	}
//	return false;
//}

//bool GRBLMaster::movABC(int axisIdx, float cmdMM) {
//	if (_flags.simuMode > 0) {
//		_state.targetXYZABC[3 + axisIdx] += cmdMM;
//		_state.action = 'R';
//		return true;
//	}
//
//	int mask = (0x01 << (axisIdx + 3));
//	vector<uint8_t> cmd = GRBLParser::getCmdGoToRltPos(
//		0, 0, 0,
//		cmdMM, cmdMM, cmdMM,
//		mask);
//	return  _core.sendMovCheckReply(cmd);
//}

bool GRBLMaster::movAxisByDist(int axisIdx, float distance, float mmPerMin) {
	int mask = (0x00000001 << axisIdx);

	vector<uint8_t> cmd;
	if (mmPerMin < 0) {
		cmd = GRBLParser::getCmdMoveDistance(axisIdx, distance);
	}
	else {
		cmd = GRBLParser::getCmdMoveDistanceAtRate(axisIdx, distance, mmPerMin);
	}

	if (_flags.simuMode > 0) {
		_state.targetXYZABC[axisIdx] += distance;
		_state.action = 'R';
		_core.simuSend(cmd);
		return true;
	}
	return  _core.sendMovCheckReply(cmd);
}

bool GRBLMaster::movAxisesByDist(int mask, std::vector<float> distances, float mmPerMin) {
	for (int axisIdx = 0; axisIdx < _state.parsedAxisNum; axisIdx++) {
		int aMask = (0x00000001 << axisIdx);
		if ((aMask & mask) == aMask) {
			_state.targetXYZABC[axisIdx] += distances[axisIdx];
		}
	}

	vector<uint8_t> cmd;
	if (mmPerMin > 0) {
		cmd = GRBLParser::getCmdMoveDistanceAtRate(distances, mask, mmPerMin);
	}
	else {
		cmd = GRBLParser::getCmdMoveDistance(distances, mask);
	}
	if (_flags.simuMode > 0) {
		_state.action = 'R';
		_core.simuSend(cmd);
		return true;
	}
	return  _core.sendMovCheckReply(cmd);
}

bool GRBLMaster::movAxisesToPosition(int axisMask, std::vector<float> positions, float mmPerMin) {
	for (int axisIdx = 0; axisIdx < _state.parsedAxisNum; axisIdx++) {
		int aMask = (0x0001 << axisIdx);
		if ((aMask & axisMask) == aMask) {
			_state.targetXYZABC[axisIdx] = positions[axisIdx];
		}
	}

	vector<uint8_t> cmd = movAxisesToPositionBuildCmd(axisMask, positions, mmPerMin);

	if (_flags.simuMode > 0) {
		_state.action = 'R';
		_core.simuSend(cmd);
		return true;
	}
	return  _core.sendMovCheckReply(cmd);
}

std::vector<uint8_t> GRBLMaster::movAxisesToPositionBuildCmd(int axisMask, std::vector<float> positions, float mmPerMin) {
	vector<uint8_t> cmd;
	if (mmPerMin > 0) {
		cmd = GRBLParser::getCmdGoToWorkPosAtRate(positions, axisMask, mmPerMin);
	}
	else {
		cmd = GRBLParser::getCmdGoToWorkPos(positions, axisMask);
	}
	return cmd;
}

bool GRBLMaster::movAxisesToTargetPosition(int axisMask, float mmPerMin) {
	vector<uint8_t> cmd;
	if (mmPerMin > 0) {
		cmd = GRBLParser::getCmdGoToWorkPosAtRate(_state.getTarget(), axisMask, mmPerMin);
	}
	else {
		cmd = GRBLParser::getCmdGoToWorkPos(_state.getTarget(), axisMask);
	}
	return  _core.sendMovCheckReply(cmd);
}

bool GRBLMaster::movPlatform(float hrzMMWithSign, float vtcMMWithSign) {
	vector<uint8_t> cmd = GRBLParser::getCmdMoveDistance(true, hrzMMWithSign, true, vtcMMWithSign, false, 0);

	return  _core.sendMovCheckReply(cmd);
}

bool GRBLMaster::movPlatform(float cmdHrzMM, float cmdVtcMM, float cmdPerpenMM) {
	vector<uint8_t> cmd = GRBLParser::getCmdMoveDistance(true, cmdHrzMM, true, cmdVtcMM, true, cmdPerpenMM);

	return  _core.sendMovCheckReply(cmd);
}

int GRBLMaster::stopPlatform() {
	if (_core.slaveNativeHandle == NULL) {
		return -1;
	}
	bool ret = true;
	// send stop command
	//flagShouldBeBuzy = false;
	vector<uint8_t> stopCmd = GRBLParser::getCmdStop();
	ret = _core.sendCmdCore(stopCmd);
	if (!ret) {
		return -1;
	}

	if (_flags.simuMode > 0) {
		_state.action = 'I';
		return 1;
	}

	vector<uint8_t> stopReply = _core.receiveRplyCore(GRBLParser::getReplyStop(), platformGeneralCmdReplyTimeOut);

	vector<uint8_t> unlockCmd = GRBLParser::getCmdUnlock();
	ret = _core.sendCmdCore(unlockCmd);
	if (!ret) {
		return -1;
	}
	vector<uint8_t> unlockReply = _core.receiveRplyCore(GRBLParser::getReplyUnlocked(), platformGeneralCmdReplyTimeOut);

	purge();
	//Sleep(stateQuarryReplyTimeOut);
	getPlatformState();

	return _state.action == 'I';
}

bool GRBLMaster::isPlatformStopped(int delayMSAfterMov) {
	getPlatformStateReceiveCore();
	bool stateUpdateSuccess = getPlatformState();
	if (!stateUpdateSuccess) {
		return false;
	}
	return _state.action != GRBL_RUN;
}

bool GRBLMaster::waitUntilPlatformStopIfNeeded(bool flagWaitUntilStop, int delayMSAfterMov) {
	if (!flagWaitUntilStop) {
		return true;
	}
	bool stateUpdateSuccess = getPlatformState();
	while (stateUpdateSuccess && (_state.action == 'R')) {
		Sleep(100);
		stateUpdateSuccess = getPlatformState();
	}

	if (delayMSAfterMov > 0) {
		Sleep(delayMSAfterMov);
	}

	return true;
}

bool GRBLMaster::movPlatformPerpendicular(float mmWithSign) {
	if (_flags.simuMode > 0) {
		_state.targetXYZABC[2] += mmWithSign;
		_state.action = 'R';
		return true;
	}
	vector<uint8_t> cmd = GRBLParser::getCmdMoveDistance(false, 0, false, 0, true, mmWithSign);

	return  _core.sendMovCheckReply(cmd);
}

bool GRBLMaster::movPlatformPerpendicularAbs(float mmWithSign) {
	vector<uint8_t> cmd = GRBLParser::getCmdGoToWorkPos(false, 0, false, 0, true, mmWithSign);

	return  _core.sendMovCheckReply(cmd);
}

bool GRBLMaster::movPlatformEx(char exAxisName, float cmdMM) {
	return false;
}


// return is moving
bool updateAxisAtRate(float* cur, float* tar, float multiplier, float* mmPerSec, float mmPerSecSec, clock_t t0) {
	if (*cur != *tar) {
		float timeDiffS = (clock() - t0) / 1000.0f;
		if (t0 == 0) {
			timeDiffS = 0;
		}
		else {
			timeDiffS = min(1, timeDiffS);
		}
		float dist = timeDiffS * mmPerSec[0];
		float vInc = mmPerSecSec * timeDiffS * multiplier;
		*mmPerSec += vInc;
		float distDiff = *tar - *cur;

		//TLogger::writeLine("DeltaT=" + TFormater::float2str(timeDiffS, -1, 3)
		//	+ ", inc=" + TFormater::float2str(vInc, -1, 3)
		//	+ ", speed=" + TFormater::float2str(*mmPerSec, -1, 3));

		//string rates = TFormater::float32str(timeDiffS, distDiff, rate);
		//TLogger::w1riteLine("Update" + rates);
		if (abs(distDiff) < dist) {
			*cur = *tar;
		}
		else {
			*cur = *cur + dist * (distDiff > 0 ? 1 : -1);
		}
		return true;
	}
	else {
		return false;
	}
}

bool setSimuHomePos(float* axises, int mask, clock_t time, clock_t endTime, float ratePerSec) {
	if (time >= endTime) {
		return false;
	}
	else {
		float pos = (endTime - time) / 1000.0f * ratePerSec;
		for (int idx = 0; mask != 0; idx++) {
			if ((mask & 0x01) == 0x01) {
				axises[idx] = pos;
			}
			mask = (mask >> 1);
		}
		return true;
	}
}

int GRBLMaster::simuUpdate() {
	if (_state.simuHomingEndTime > 0) {
		auto time = TStopWatch::getRealTimeSinceStart();
		TLogger::writeLine("Check simu home " + TFormater::int2str(time) + " -> " + TFormater::int2str(_state.simuHomingEndTime));
		if (!setSimuHomePos(_state.workPosXYZABC, _state.simuHomeMask, time, _state.simuHomingEndTime, _flags.simuRateMMPerSecX)) {
			_state.homed();
		}
	}
	else {
		bool anyMov = false;
		float oldPos[6];
		memcpy(oldPos, _state.workPosXYZABC, sizeof(oldPos));
		for (int idx = 0; idx < sizeof(_state.workPosXYZABC) / sizeof(float); idx++) {
			anyMov |= updateAxisAtRate(
				_state.workPosXYZABC + idx,
				_state.targetXYZABC + idx,
				_flags.rateMultiplier,
				&_flags.simuRateMMPerSecX,
				_flags.simuAccMMPerSecSec,
				_state.updateTime);
		}
		_state.updateTime = clock();
		if (!anyMov) {
			_state.action = 'I';
			_flags.simuRateMMPerSecX = 0;
			_state.updateTime = 0;
			memcpy(_state.lastIdleXYZABC, _state.workPosXYZABC, sizeof(oldPos));
		}
		else {
			_state.action = 'R';
			_state.progress = calProgress(_state.lastIdleXYZABC, _state.workPosXYZABC, _state.targetXYZABC, 6);
		}
	}

	string simuReply = "";
	if (_state.action == 'I') {
		simuReply = "Idle,";
	}
	else if (_state.action == 'R') {
		simuReply = "Run,";
	}
	else if (_state.action == 'H') {
		simuReply = "Home,";
	}

	simuReply += TFormater::floats2str(_state.workPosXYZABC, _state.parsedAxisNum, -3, 3);
	_core.simuReceive(simuReply);

	return 1;
}

bool GRBLMaster::activateAxises(int axisMask, int axisNumMax) {
	int byteNum = axisNumMax / 8;
	if (axisNumMax / 8 * 8 < axisNumMax) {
		byteNum++;
	}

	string commandStr = "activate 0x";
	commandStr += TFormater::int2HexStr(axisMask, byteNum);
	commandStr += "\n";
	auto cmd = TFormater::str2u8(commandStr);
	string replyStart = "Activated:";

	if (_flags.simuMode > 0) {
		if (_core.simuSend(cmd)) {
			return _core.simuReceive(replyStart);
		}
	}
	else {
		if (_core.sendCmdCore(cmd)) {
			string replyStart = "Activated:";
			auto reply = _core.receiveRplyCore(replyStart, platformGeneralCmdReplyTimeOut);
			return reply.size() > 0;
		}
	}
	return false;
}

bool GRBLMaster::getPlatformStateQuarry() {
	return  _core.sendCmdCore(GRBLParser::getCmdStateQuarry());
}
//
//bool GRBLMaster::getPlatformStateReceive(std::string* stateToPrint, CtrlMovState* ctrlStateOut,
//	float* machinePosXOut, float* machinePosYOut, float* machinePosZOut, float* machinePosAOut,
//	float* workPosXOut, float* workPosYOut, float* workPosZOut, float* workPosAOut,
//	int* xLimOut, int* yLimOut, int* zLimOut) {
//	vector<uint8_t> stateReply = _core.receiveRplyCore("", stateQuarryReplyTimeOut);
//	while (TFormater::StrLikeU8(stateReply).compare(GRBLParser::getGeneralReply()) == 0) {
//		// ignore ok's from other commands
//		stateReply = _core.receiveRplyCore("", stateQuarryReplyTimeOut);
//	}
//	if (TFormater::StrLikeU8(stateReply).compare(GRBLParser::getReplyHardLimitedIdle()) == 0) {
//		ProcS::setIfNotNullPtr<CtrlMovState>(CtrlMovState::alarmTriggered, ctrlStateOut);
//		return true;
//	}
//	if (TFormater::StrLikeU8(stateReply).compare(GRBLParser::getReplyHardLimitedRun()) == 0) {
//		ProcS::setIfNotNullPtr<CtrlMovState>(CtrlMovState::alarmTriggered, ctrlStateOut);
//		return true;
//	}
//	if (stateReply.size() == 0) {
//		return false;
//	}
//	else {
//		bool flagStateAlarm = GRBLParser::parseIsAlarm(stateReply);
//		bool flagStateIdle = GRBLParser::parseIsIdle(stateReply);
//		bool flagstateMoving = GRBLParser::parseIsMoving(stateReply);
//		bool flagStateHoming = GRBLParser::parseIsHoming(stateReply);
//		bool flagFalseAlarm = false;
//		vector<float> stateMachinePos = GRBLParser::parseMachinePos(stateReply);
//		vector<float> stateWorkPos = GRBLParser::parseWorkPos(stateReply);
//		vector<int> hardLimit;
//		//if (platformLagacyVer == 1) {
//		//	hardLimit = GRBLParser::parseHardLimitLegacy0901(stateReply);
//		//}
//		//else {
//		hardLimit = GRBLParser::parseHardLimit(stateReply);
//		//}
//
//		ProcS::setIfNotNullPtr<string>(GRBLParser::toDisplayable(stateReply), stateToPrint);
//		if (flagStateAlarm) {
//			ProcS::setIfNotNullPtr<CtrlMovState>(CtrlMovState::alarm, ctrlStateOut);
//		}
//		else if (flagStateIdle) {
//			ProcS::setIfNotNullPtr<CtrlMovState>(CtrlMovState::idle, ctrlStateOut);
//		}
//		else if (flagstateMoving) {
//			ProcS::setIfNotNullPtr<CtrlMovState>(CtrlMovState::run, ctrlStateOut);
//		}
//		else if (flagStateHoming) {
//			ProcS::setIfNotNullPtr<CtrlMovState>(CtrlMovState::homing, ctrlStateOut);
//		}
//		else {
//			ProcS::setIfNotNullPtr<CtrlMovState>(CtrlMovState::undefined, ctrlStateOut);
//		}
//
//		if (stateMachinePos.size() >= 3) {
//			ProcS::setIfNotNullPtr<float>(stateMachinePos[0], machinePosXOut);
//			ProcS::setIfNotNullPtr<float>(stateMachinePos[1], machinePosYOut);
//			ProcS::setIfNotNullPtr<float>(stateMachinePos[2], machinePosZOut);
//		}
//		if (stateMachinePos.size() >= 4) {
//			ProcS::setIfNotNullPtr<float>(stateMachinePos[3], machinePosAOut);
//		}
//		if (stateWorkPos.size() >= 3) {
//			ProcS::setIfNotNullPtr<float>(stateWorkPos[0], workPosXOut);
//			ProcS::setIfNotNullPtr<float>(stateWorkPos[1], workPosYOut);
//			ProcS::setIfNotNullPtr<float>(stateWorkPos[2], workPosZOut);
//		}
//		if (stateWorkPos.size() >= 4) {
//			ProcS::setIfNotNullPtr<float>(stateWorkPos[3], workPosAOut);
//		}
//		if (hardLimit.size() >= 3) {
//			ProcS::setIfNotNullPtr<int>(hardLimit[0], xLimOut);
//			ProcS::setIfNotNullPtr<int>(hardLimit[1], yLimOut);
//			ProcS::setIfNotNullPtr<int>(hardLimit[2], zLimOut);
//		}
//
//		bool flagStateCheck = (flagStateAlarm || flagStateIdle || flagstateMoving || flagStateHoming);
//		if (!flagStateCheck) {
//			TLogger::writeWarnning("[GRBLMaster] State check failed");
//		}
//		bool machinePosCheck = (stateMachinePos.size() >= 3);
//		if (!machinePosCheck) {
//			TLogger::writeWarnning("[GRBLMaster] Machine pos check failed");
//		}
//		bool workPosCheck = (stateWorkPos.size() >= 3);
//		if (!workPosCheck) {
//			TLogger::writeWarnning("[GRBLMaster] Work pos check failed");
//		}
//
//		bool ret = flagStateCheck && machinePosCheck && workPosCheck;
//
//		return ret;
//	}
//}

bool GRBLMaster::getPlatformStateReceive(char* stateOut,
	int coordNum, float* workPosesOut,
	int* allLimOut) {
	bool ret = getPlatformStateReceiveCore();

	ProcS::setIfNotNullPtr<char>(_state.action, stateOut);
	for (int idx = 0; idx < min(sizeof(_state.workPosXYZABC) / sizeof(float), coordNum); idx++) {
		ProcS::setIfNotNullPtr<float>(_state.workPosXYZABC[idx], workPosesOut + idx);
	}
	ProcS::setIfNotNullPtr<int>(_state.limits, allLimOut);

	return ret;
}

bool GRBLMaster::getPlatformState(char* stateOut,
	int coordNum, float* workPosesOut,
	int* allLimOut) {
	if (getPlatformStateQuarry()) {
		if (_flags.simuMode > 0) {
			return simuUpdate();
		}
		bool ret = getPlatformStateReceive(stateOut,
			coordNum, workPosesOut,
			allLimOut);
		return ret;
	}
	else {
		return false;
	}
}

bool GRBLMaster::getPlatformStateReceiveCore() {
	//TLogger block("GRBL rcv");
	for (int attemptIdx = 0; attemptIdx < _flags.receiveAttemptAllowance; attemptIdx++) {
		if (attemptIdx == 1) {
			int stop = 1;
		}
		bool lastTry = (attemptIdx == _flags.receiveAttemptAllowance - 1);
		vector<uint8_t> stateReply = _core.receiveRplyCore("", stateQuarryReplyTimeOut);
		while (TFormater::StrLikeU8(stateReply).compare(GRBLParser::getGeneralReply()) == 0) {
			// ignore ok's from other commands
			stateReply = _core.receiveRplyCore("", stateQuarryReplyTimeOut);
		}
		//TLogger::writeLine("trans end");
		if (stateReply.size() == 0) {
			_state.action = GRBL_UNDEFINED;
			return false;
		}

		if (isEspNowNoSend(stateReply)) {
			TLogger::writeLine("[GRBLMaster] Encounter ESP Now Err. Will retry.");
			continue;
		}

		if (TFormater::StrLikeU8(stateReply).compare(GRBLParser::getReplyHardLimitedIdle()) == 0) {
			_state.action = GRBL_ALARM_TRIGGERED;
			return true;
		}
		if (TFormater::StrLikeU8(stateReply).compare(GRBLParser::getReplyHardLimitedRun()) == 0) {
			_state.action = GRBL_ALARM_TRIGGERED;
			return true;
		}

		//TLogger block("parse");
		bool flagStateAlarm = GRBLParser::parseIsAlarm(stateReply);
		bool flagStateIdle = GRBLParser::parseIsIdle(stateReply);
		bool flagstateMoving = GRBLParser::parseIsMoving(stateReply);
		bool flagStateHoming = GRBLParser::parseIsHoming(stateReply);
		//bool flagFalseAlarm = false;

		vector<float> stateMachinePos = GRBLParser::parseMachinePos(stateReply);
		vector<float> stateWorkPos = GRBLParser::parseWorkPos(stateReply);
		vector<int> missingAxis = GRBLParser::parseMissingAxises(stateReply);
		_state.limits = GRBLParser::parseHardLimitInt(stateReply);
		_state.sensors = GRBLParser::parseSensors(stateReply);
		_state.parsedAxisNum = stateWorkPos.size();
		if (flagStateAlarm) {
			_state.action = GRBL_ALARM;
		}
		else if (flagStateIdle) {
			_state.action = GRBL_IDLE;
		}
		else if (flagstateMoving) {
			_state.action = GRBL_RUN;
		}
		else if (flagStateHoming) {
			_state.action = GRBL_HOME;
		}
		else {
			_state.action = GRBL_UNDEFINED;
		}

		for (int idx = 0; idx < stateMachinePos.size(); idx++) {
			_state.machPosXYZABC[idx] = stateMachinePos[idx];
		}
		for (int idx = 0; idx < stateWorkPos.size(); idx++) {
			_state.workPosXYZABC[idx] = stateWorkPos[idx];
		}

		if (!flagstateMoving) {
			memcpy(_state.lastIdleXYZABC, _state.workPosXYZABC, sizeof(_state.workPosXYZABC));
			memcpy(_state.targetXYZABC, _state.workPosXYZABC, sizeof(_state.workPosXYZABC));
		}
		else {
			_state.progress = calProgress(_state.lastIdleXYZABC, _state.workPosXYZABC, _state.targetXYZABC, 6);
		}

		bool flagStateCheck = (flagStateAlarm || flagStateIdle || flagstateMoving || flagStateHoming);
		if (!flagStateCheck) {
			if (lastTry) {
				TLogger::writeWarnning("[GRBLMaster] State check failed");
			}
			else {
				TLogger::writeLine("[GRBLMaster] State check failed. Will retry");
			}
		}
		bool machinePosCheck = (stateMachinePos.size() >= 1);
		if (!machinePosCheck) {
			if (lastTry) {
				TLogger::writeWarnning("[GRBLMaster] Machine pos check failed");
			}
			else {
				TLogger::writeLine("[GRBLMaster] Machine check failed. Will retry");
			}
		}
		bool workPosCheck = (stateWorkPos.size() >= 1);
		if (!workPosCheck) {
			if (lastTry) {
				TLogger::writeWarnning("[GRBLMaster] Work pos check failed");
			}
			else {
				TLogger::writeLine("[GRBLMaster] Work check failed. Will retry");
			}
		}

		bool ret = flagStateCheck && machinePosCheck && workPosCheck;
		if (!ret) {
			TLogger::writeLine("[GRBLMaster] State Raw reply: " + TFormater::toDisplayable(stateReply));
		}
		else {
			if (attemptIdx > 0) {
				TLogger::writeLine("[GRBLMaster] Retry success on " + TFormater::int2str(attemptIdx));
			}
			return ret;
		}
	}
	_state.action = GRBL_UNDEFINED;
	return false;
}
//
//bool GRBLMaster::getPlatformState(std::string* stateToPrint, CtrlMovState* ctrlStateOut,
//	float* machinePosXOut, float* machinePosYOut, float* machinePosZOut, float* machinePosAOut,
//	float* workPosXOut, float* workPosYOut, float* workPosZOut, float* workPosAOut,
//	int* xLimOut, int* yLimOut, int* zLimOut) {
//	if (getPlatformStateQuarry()) {
//		bool ret = getPlatformStateReceive(stateToPrint, ctrlStateOut,
//			machinePosXOut, machinePosYOut, machinePosZOut, machinePosAOut,
//			workPosXOut, workPosYOut, workPosZOut, workPosAOut,
//			xLimOut, yLimOut, zLimOut);
//		return ret;
//	}
//	else {
//		return false;
//	}
//}

bool GRBLMaster::startHoming(int axisMask) {
	for (int axIdx = 0; axIdx < _state.MaxAxNum; axIdx++) {
		uint32_t bit = 0b01;
		bit = bit << axIdx;
		if (((axisMask & bit) == bit) || ((axisMask & _state.FullAxMask) == 0)) {
			_state.targetXYZABC[axIdx] = 0;
		}
	}

	if (_flags.simuMode > 0) {
		_state.action = 'H';
		_state.simuHomingEndTime = TStopWatch::getRealTimeSinceStart() + 2000;
		_state.simuHomeMask = (axisMask == 0) ? _state.FullAxMask : axisMask;
		_flags.simuRateMMPerSecX = _flags.simuAccMMPerSecSec;

		_core.simuSend(GRBLParser::getCmdEnableHoming(true));
		_core.simuSend(GRBLParser::getCmdConfigQuarry());
		_core.simuSend(GRBLParser::getCmdStartHoming(axisMask, _state.FullAxMask));
		return true;
	}
	bool ret = true;

	// homing
	if (_platformSlaveConfig.hasEntry(22)) {
		ret &= _core.sendCmdCheckReply(GRBLParser::getCmdEnableHoming(true),
			platformGeneralCmdReplyTimeOut, GRBLParser::getConfigReply(),
			EspNowDelayHint);
	}

	if (ret) {
		ret &= updateConfigFromSlave();
	}
	if (ret) {
		ret &= _core.sendCmdCore(startHomingBuildCmd(axisMask));
	}
	_state.action = 'H';

	//if (ret && (platformLagacyVer == 1)) {
	//	flagShouldBeBuzyX = true;
	//	writeLine("startHoming set flagShouldBeBuzy");
	//}

	return ret;
}

bool GRBLMaster::isHomingCompleteLagacy() {
	return  _core.sendCmdCheckReply(GRBLParser::noCmd(), platformGeneralCmdReplyTimeOut, GRBLParser::getGeneralReply());
}

std::vector<uint8_t> GRBLMaster::startHomingBuildCmd(int axisMask) {
	return GRBLParser::getCmdStartHoming(axisMask, _state.FullAxMask);
}

int GRBLMaster::confirmPosition(int axisMask, std::vector<float> coordinates, float errorAllowance) {
	int axisIdx = 0;
	while (axisMask > 0) {
		if ((axisMask & 0b01) == 0b01) {
			if (abs(_state.targetXYZABC[axisIdx] - coordinates[axisIdx]) > errorAllowance) {
				return 0;
			}
		}
		axisMask = (axisMask >> 1);
		axisIdx++;
	}
	if (_state.parsedAxisNum < axisIdx) {
		return 0;
	}
	return 1;
}

bool GRBLMaster::finishHoming() {
	bool ret = true;
	//if ( _core.sendCmdCheckReply(GRBLParser::noCmd(), platformGeneralCmdReplyTimeOut, GRBLParser::getGeneralReply())) {
	//flagShouldBeBuzyX = false;
	if (_platformSlaveConfig.hasEntry(22)) {
		ret &= _core.sendCmdCheckReply(GRBLParser::getCmdEnableHoming(false),
			platformGeneralCmdReplyTimeOut, GRBLParser::getConfigReply());
	}
	ret &= _core.sendCmdCheckReply(GRBLParser::getCmdEnableHardLimit(),
		platformGeneralCmdReplyTimeOut, GRBLParser::getConfigReply());
	//ret &= softRebootPlatform();

	ret &= updateConfigFromSlave();

	return ret;
	//}
	//return false;
}

bool GRBLMaster::switchToCoordSystem(int coordIdx) {
	if (SlaveComm->isConnected()) {
		_core.platformCoordSystemIdx = coordIdx;
		return  _core.sendCmdCheckReply(GRBLParser::getCmdSwitchToCoord(coordIdx + 1),
			platformGeneralCmdReplyTimeOut, GRBLParser::getGeneralReply());
	}
	else {
		return false;
	}
}

bool GRBLMaster::setCoordPosition(int coordIdx,
	float curX,
	float curY,
	float curZ,
	float curA,
	float curB,
	float curC,
	int mask) {
	if (getPlatformState()) {
		float offsetX = _state.machPosXYZABC[0] - curX;
		float offsetY = _state.machPosXYZABC[1] - curY;
		float offsetZ = _state.machPosXYZABC[2] - curZ;
		float offsetA = _state.machPosXYZABC[3] - curA;
		float offsetB = _state.machPosXYZABC[4] - curB;
		float offsetC = _state.machPosXYZABC[5] - curC;
		vector<uint8_t> cmd = GRBLParser::getCmdSetCoord(coordIdx + 1,
			offsetX, offsetY, offsetZ,
			offsetA, offsetB, offsetC, mask);

		string cmdStr = TFormater::toDisplayable(cmd);

		return  _core.sendCmdCheckReply(cmd,
			platformGeneralCmdReplyTimeOut, GRBLParser::getGeneralReply());
	}
	return false;
}

GRBLMaster::Core::~Core() {
	if (isHandleOwner) {
		if (slaveNativeHandle != nullptr) {
			delete slaveNativeHandle;
		}
	}
}

int GRBLMaster::Core::getCoordSystemIdx() {
	return platformCoordSystemIdx;
}

time_t GRBLMaster::getPlatformUnavailableDuration() {
	return SlaveComm->getUnavailableDuration();
}

void GRBLMaster::takeControl() {
	while (_flags.flagBuzy) {
		Sleep(5);
	}
	_flags.flagBuzy = true;
}
void GRBLMaster::releaseControl() {

}
//bool GRBLMaster::isLagacyWithLLLimitCheck() {
//	return false;
//}
//
//bool GRBLMaster::isLagacyWithSilentHoming() {
//	return false;
//}
//
//bool GRBLMaster::isLagacyCheckLimitEdge() {
//	return false;
//}


bool  GRBLMaster::Core::sendCmdCore(vector<uint8_t> cmd) {
	if (cmd.size() == 0) {
		return true;
	}

	if (slaveNativeHandle == NULL) {
		return false;
	}

	if (!((SerialPortManager*)slaveNativeHandle)->isConnected()) {
		TLogger::writeWarnning("GRBLMaster not connected");
		return false;
	}

	if (((SerialPortManager*)slaveNativeHandle)->send(&cmd[0], cmd.size())) {
		if (sentCB != NULL) {
			sentCB(cmd);
		}
		return true;
	}
	else {
		TLogger::writeLineX("Cannot send: {" + GRBLParser::toDisplayable(cmd) + "}", false, true);
		return false;
	}
}

std::vector<uint8_t>  GRBLMaster::Core::peekRply() {
	vector<uint8_t> ret = ((SerialPortManager*)slaveNativeHandle)->peakBytes();
	//TLogger::writeLineX("Peak platform reply: {" + GRBLParser::toDisplayable(ret) + "}", debug_flagSerialPortCommToLog);
	return ret;
}

std::vector<uint8_t>  GRBLMaster::Core::receiveRplyCore(std::vector<uint8_t> expReply, int timeout, string discardErrorHint) {
	vector<uint8_t> delim = GRBLParser::getReplyDelim();
	vector<int> replySegPoses = SerialPortManager::find(&expReply[0], expReply.size(), &delim[0], delim.size());

	if (replySegPoses.size() == 0) {
		TLogger::writeWarnning(" _core.receiveRplyCore: no delim in the reply");
		return GRBLParser::noCmd();
	}

	if (timeout < 0) {
		timeout = platformGeneralCmdReplyTimeOut;
	}
	std::vector<uint8_t> reply = ((SerialPortManager*)slaveNativeHandle)->receiveBytes(
		"", vector<uint8_t>(), "", expReply, timeout);
	if ((discardErrorHint.size() > 0) && (TFormater::beginWith(reply, discardErrorHint))) {
		TLogger::writeLine("Discard error hint " + TFormater::toDisplayable(reply));
		Sleep(250);
		reply = ((SerialPortManager*)slaveNativeHandle)->receiveBytes(
			"", vector<uint8_t>(), "", expReply, timeout);
	}
	if (reply.size() > 0) {
		if (replyCB != NULL) {
			replyCB(reply);
		}
	}
	else {
		if (timeoutCB != NULL) {
			timeoutCB(expReply);
		}
	}
	return reply;
}

std::vector<uint8_t>  GRBLMaster::Core::receiveRplyCore(std::string expReply, int timeout) {
	//TFormater::StrLikeU8 replyContent(expReply);
	std::vector<uint8_t> content = ((SerialPortManager*)slaveNativeHandle)->receiveBytes(
		expReply, vector<uint8_t>(), "", GRBLParser::getReplyDelim(), timeout);
	//while (replyContent.compare(content) == 0) {
	//	content = _core.slaveManager->receiveBytes(
	//		expReply, vector<uint8_t>(), "", GRBLParser::getReplyDelim(), timeout);
	//}
	if (content.size() > 0) {
		if (replyCB != NULL) {
			replyCB(content);
		}
	}
	else {
		if (timeoutCB != NULL) {
			timeoutCB(GRBLParser::noCmd());
		}
	}
	return content;
}

bool  GRBLMaster::Core::sendCmdCheckReply(std::vector<uint8_t> cmd, int timeout, std::vector<uint8_t> expectedReply, string discardErrorHint) {
	if (sendCmdCore(cmd)) {
		std::vector<uint8_t> reply = receiveRplyCore(expectedReply, timeout, discardErrorHint);
		if (reply.size() > 0) {
			return  SerialPortManager::compareBuffers(&reply[0], reply.size(), &expectedReply[0], expectedReply.size()) >= 0;
		}
	}
	return false;
}

bool  GRBLMaster::Core::sendResetCmdCheckReply(bool flagInit, int timeout) {
	//if ((platformLagacyVer != 1) || (!flagInit)) {
	sendCmdCore(GRBLParser::getCmdReset());
	//}
	std::vector<uint8_t> expectedReply;
	//if (platformLagacyVer == 1) {
	//	expectedReply = GRBLParser::getReplyResetLegacy();
	//}
	//else {
	expectedReply = GRBLParser::getReplyReset();
	//}
	std::vector<uint8_t> reply = receiveRplyCore(expectedReply, timeout);
	((SerialPortManager*)slaveNativeHandle)->purge();

	return reply.size() > 0
		&& (SerialPortManager::compareBuffers(&reply[0], reply.size(), &expectedReply[0], expectedReply.size()) >= 0);
}

void GRBLMaster::State::homed() {
	action = 'I';

	for (int bitIdx = 0; bitIdx < parsedAxisNum; bitIdx++) {
		int mask = (0x01) << bitIdx;
		if ((mask & simuHomeMask) != 0) {
			machPosXYZABC[bitIdx] = 0;
			workPosXYZABC[bitIdx] = 0;
			targetXYZABC[bitIdx] = 0;
		}
	}

	limits = 0;
	simuHomingEndTime = 0;
	updateTime = 0;
	simuHomeMask = 0;
}

std::vector<float> GRBLMaster::State::getTarget() {
	vector<float> ret;

	for (int idx = 0; idx < parsedAxisNum; idx++) {
		ret.push_back(targetXYZABC[idx]);
	}

	return ret;
}

bool  GRBLMaster::Core::sendMovCheckReply(std::vector<uint8_t> cmd) {
	if (sendCmdCore(cmd)) {
		std::vector<uint8_t> reply = receiveRplyCore(GRBLParser::getGeneralReply(), goToCmdReplyTimeOut);
		std::vector<uint8_t> expReply = GRBLParser::getGeneralReply();
		return reply.size() > 0 && (SerialPortManager::compareBuffers(&reply[0], reply.size(), &expReply[0], expReply.size()) >= 0);
	}
	else {
		return false;
	}
}

bool GRBLMaster::Core::simuSend(std::vector<uint8_t> cmd) {
	sendCmdCore(cmd);
	return true;
}

bool GRBLMaster::Core::simuReceive(std::string reply) {
	((SerialPortManager*)slaveNativeHandle)->simuReceive(reply);

	return true;
}

bool GRBLMaster::PlatformConfig::getConfig(std::vector<std::string>& paraCodesOut, std::vector<std::string>& paraValsOut) {
	paraCodesOut = paraCodeStrs;
	paraValsOut = paraValStrs;
	return true;
}

int find(unsigned char target, std::vector<uint8_t>& content, int offset) {
	while (offset < content.size() && content[offset] != target) {
		offset++;
	}
	return offset;
}

string truncate(std::vector<uint8_t>& content, int start, int end) {
	string strOut;
	strOut.resize(end - start + 1);
	memcpy(&strOut[0], &content[start], end - start + 1);

	return strOut;
}

bool GRBLMaster::PlatformConfig::matchConfigNSetIfNot(std::vector<uint8_t> cmd) {
	int paraNameStart = find('$', cmd, 0) + 1;
	int eqPos = find('=', cmd, paraNameStart);
	int paraNameEnd = eqPos - 1;
	int paraValStart = eqPos + 1;
	int paraValEnd = find('\r', cmd, eqPos) - 1;

	string paraCodeStr = truncate(cmd, paraNameStart, paraNameEnd);
	string paraValStr = truncate(cmd, paraValStart, paraValEnd);
	int paraCode = TFormater::str2int(paraCodeStr);
	float paraVal = TFormater::str2float(paraValStr);

	for (int idx = 0; idx < paraCodes.size(); idx++) {
		if (paraCode == paraCodes[idx]) {
			bool ret = (paraVal == TFormater::str2float(paraValStrs[idx]));
			//paraVals[idx] = paraVal;
			return ret;
		}
	}

	return false;
}

bool GRBLMaster::PlatformConfig::matchCoordNSetIfNot(int coordIdx, float xOffset, float yOffset) {
	char coordLastDigit = coordIdx + '4';
	string coordName = "G5" + coordLastDigit;
	for (int idx = 0; idx < coordNames.size(); idx++) {
		if (coordNames[idx].compare(coordName) >= 0) {
			bool ret = (coordOffsets[idx][0] == xOffset) && (coordOffsets[idx][1] == yOffset);
			coordOffsets[idx][0] = xOffset;
			coordOffsets[idx][1] = yOffset;
			return ret;
		}
	}

	return false;
}

bool GRBLMaster::PlatformConfig::matchCoordNSetIfNot(int coordIdx, float xOffset, float yOffset, float zOffset) {
	char coordLastDigit = coordIdx + '4';
	string coordName = string("G5");
	coordName += coordLastDigit;
	for (int idx = 0; idx < coordNames.size(); idx++) {
		int val = coordNames[idx].compare(coordName);
		if (coordNames[idx].compare(coordName) >= 0) {
			bool ret = (coordOffsets[idx][0] == xOffset) && (coordOffsets[idx][1] == yOffset) && (coordOffsets[idx][2] == zOffset);
			coordOffsets[idx][0] = xOffset;
			coordOffsets[idx][1] = yOffset;
			coordOffsets[idx][2] = zOffset;
			return ret;
		}
	}

	return false;
}

void GRBLMaster::PlatformConfig::setConfig(bool flagHintIncluded, std::vector<uint8_t> reply) {
	paraCodes.clear();
	//paraVals.clear();
	paraNotes.clear();
	paraCodeStrs.clear();
	paraValStrs.clear();

	int lastLineEnd = 0;
	while (true) {
		int lineStart = find('$', reply, lastLineEnd);
		if (lineStart >= reply.size()) {
			return;
		}
		int lineEnd = find(0x0a, reply, lineStart);
		int eqPos = find('=', reply, lineStart);
		int spacePos = 0;
		if (flagHintIncluded) {
			spacePos = find(' ', reply, eqPos);
		}
		else {
			spacePos = lineEnd - 1;
		}
		if (eqPos < reply.size() && lineEnd < reply.size()) {
			int idxStart = lineStart + 1;
			int idxEnd = eqPos - 1;
			int valStart = eqPos + 1;
			int valEnd = spacePos - 1;

			if (reply[idxStart] == 'N') {
				// N0 or N1, not used
			}
			else {
				string idxStr = truncate(reply, idxStart, idxEnd);
				string valStr = truncate(reply, valStart, valEnd);

				paraCodes.push_back(TFormater::str2int(idxStr));
				paraCodeStrs.push_back(idxStr);
				//paraVals.push_back(TFormater::str2float(valStr));
				paraValStrs.push_back(valStr);
			}

			if (flagHintIncluded && spacePos < reply.size()) {
				int noteStart = spacePos + 1;
				int noteEnd = lineEnd - 2;
				string noteStr = truncate(reply, noteStart, noteEnd);
				paraNotes.push_back(noteStr);
			}

			lastLineEnd = lineEnd + 1;
		}
		else {
			TLogger::writeWarnning("PlatformConfig::setConfig parsing error");
			return;
		}
	}
}

void GRBLMaster::PlatformConfig::setConfig(std::vector<std::string>& paraCodesIn, std::vector<std::string>& paraValsIn) {
	paraValStrs = paraValsIn;
	paraCodes.clear();
	paraCodeStrs.clear();
	//paraVals.clear();
	for (string paraCodeStr : paraCodesIn) {
		if (TFormater::beginWith(paraCodeStr, "$")) {
			paraCodeStr = paraCodeStr.substr(1);
		}
		paraCodeStrs.push_back(paraCodeStr);
		paraCodes.push_back(TFormater::str2int(paraCodeStr));
	}
	//for (string paraValStr : paraValStrs) {
	//	paraVals.push_back(TFormater::str2float(paraValStr));
	//}
}

int GRBLMaster::PlatformConfig::difference(PlatformConfig& currentConfig, PlatformConfig& targetConfig, PlatformConfig& changesToMatchTarget) {
	changesToMatchTarget.paraCodeStrs.clear();
	changesToMatchTarget.paraValStrs.clear();
	for (int tgtIdx = 0; tgtIdx < targetConfig.paraCodeStrs.size(); tgtIdx++) {
		for (int curIdx = 0; curIdx < currentConfig.paraCodeStrs.size(); curIdx++) {
			if (targetConfig.paraCodes[tgtIdx] == currentConfig.paraCodes[curIdx]) {
				if (targetConfig.paraValStrs[tgtIdx].compare(currentConfig.paraValStrs[curIdx]) != 0) {
					changesToMatchTarget.paraCodeStrs.push_back(targetConfig.paraCodeStrs[tgtIdx]);
					changesToMatchTarget.paraValStrs.push_back(targetConfig.paraValStrs[tgtIdx]);
				}
			}
		}
	}
	return changesToMatchTarget.paraCodeStrs.size();
}

void GRBLMaster::PlatformConfig::setCoordOffsets(std::vector<uint8_t> reply) {
	coordNames.clear();
	coordOffsets.clear();

	int lastLineEnd = 0;
	while (true) {
		int lineStart = find('[', reply, lastLineEnd);
		if (lineStart >= reply.size()) {
			return;
		}
		int p0 = find(':', reply, lineStart);
		int p1 = find(',', reply, p0);
		int p2 = find(',', reply, p1);
		int lineEnd = find(']', reply, p2);
		if (lineEnd < reply.size()) {
			int nameStart = lineStart + 1;
			int nameEnd = p0 - 1;
			int xStart = p0 + 1;
			int xEnd = p1 - 1;
			int yStart = p1 + 1;
			int yEnd = p2 - 1;
			int zStart = p2 + 1;
			int zEnd = lineEnd - 1;

			string nameStr = truncate(reply, nameStart, nameEnd);
			string xyzStr = truncate(reply, xStart, zEnd);

			coordNames.push_back(nameStr);
			coordOffsets.push_back(TFormater::str2floats(xyzStr, ','));

			lastLineEnd = lineEnd + 1;
		}
		else {
			TLogger::writeWarnning("PlatformConfig::setCoordOffsets parsing error");
			return;
		}
	}
}

//https://stadtfabrikanten.org/display/SF/Grbl+v1.1+Configuration+Manual
// GRBL config paras
/*
for CNC frame
Grbl 0.9j ['$' for help]
$0=10 (step pulse, usec)
$1=25 (step idle delay, msec)
$2=0 (step port invert mask:00000000)
$3=5 (dir port invert mask:00000101) ; this requires hard-reboot to take effect
$4=0 (step enable invert, bool)
$5=0 (limit pins invert, bool)
$6=0 (probe pin invert, bool)
$10=3 (status report mask:00000011)
$11=0.010 (junction deviation, mm)
$12=0.002 (arc tolerance, mm)
$13=0 (report inches, bool)
$20=0 (soft limits, bool)
$21=1 (hard limits, bool)
$22=1 (homing cycle, bool)
$23=7 (homing dir invert mask:00000111)
$24=30.000 (homing feed, mm/min)
$25=240.000 (homing seek, mm/min)
$26=250 (homing debounce, msec)
$27=1.000 (homing pull-off, mm)
$100=400.000 (x, step/mm)
$101=400.000 (y, step/mm)
$102=400.000 (z, step/mm)
$110=500.000 (x max rate, mm/min)
$111=500.000 (y max rate, mm/min)
$112=500.000 (z max rate, mm/min)
$120=10.000 (x accel, mm/sec^2)
$121=10.000 (y accel, mm/sec^2)
$122=10.000 (z accel, mm/sec^2)
$130=200.000 (x max travel, mm)
$131=200.000 (y max travel, mm)
$132=200.000 (z max travel, mm)
*/

/*
for Squid (A4988:16 microStep)
Grbl 0.9j ['$' for help]
$0=10 (step pulse, usec)
$1=25 (step idle delay, msec)
$2=0 (step port invert mask:00000000)
$3=5 (dir port invert mask:00000101) ; may change
$4=0 (step enable invert, bool) may change
$5=0 (limit pins invert, bool)
$6=0 (probe pin invert, bool)
$10=3 (status report mask:00000011)
$11=0.010 (junction deviation, mm)
$12=0.002 (arc tolerance, mm)
$13=0 (report inches, bool)
$20=0 (soft limits, bool)
$21=1 (hard limits, bool)
$22=1 (homing cycle, bool)
$23=0 (homing dir invert mask:00000000)
$24=30.000 (homing feed, mm/min)
$25=240.000 (homing seek, mm/min)
$26=250 (homing debounce, msec)
$27=1.000 (homing pull-off, mm)
$100=1600.000 (x, step/mm)
$101=1600.000 (y, step/mm)
$102=10498.687 (z, step/mm)  (16*200/0.3048)
$110=600.000 (x max rate, mm/min)
$111=600.000 (y max rate, mm/min)
$112=120.000 (z max rate, mm/min)
$120=10.000 (x accel, mm/sec^2)
$121=10.000 (y accel, mm/sec^2)
$122=1.000 (z accel, mm/sec^2)
$130=200.000 (x max travel, mm)
$131=200.000 (y max travel, mm)
$132=200.000 (z max travel, mm)
*/

/*
for TdrGRBL (TMC2209:64 microStep for xy, 16microStep for z)
Grbl 0.9j ['$' for help]
$0=3 (step pulse, usec)
$1=25 (step idle delay, msec)
$2=0 (step port invert mask:00000000)
$3=6 (dir port invert mask:00000101) ; may change, especially z
$4=0 (step enable invert, bool) may change
$5=0 (limit pins invert, bool)
$6=0 (probe pin invert, bool)
$10=2 (status report mask:00000011)
$11=0.010 (junction deviation, mm)
$12=0.002 (arc tolerance, mm)
$13=0 (report inches, bool)
$20=0 (soft limits, bool)
$21=1 (hard limits, bool)
$22=1 (homing cycle, bool)
$23=7 (homing dir invert mask:00000000)
$24=30.000 (homing feed, mm/min)
$25=240.000 (homing seek, mm/min)
$26=250 (homing debounce, msec)
$27=1.000 (homing pull-off, mm)
$100=3200.000 (x, step/mm)
$101=3200.000 (y, step/mm)
$102=10498.688 (z, step/mm)  (16*200/0.3048)
$103...105=NA
$110=500.000 (x max rate, mm/min)
$111=500.000 (y max rate, mm/min)
$112=500.000 (z max rate, mm/min)
$113...115=NA
$120=10.000 (x accel, mm/sec^2)
$121=10.000 (y accel, mm/sec^2)
$122=1.000 (z accel, mm/sec^2)
$123...125=NA
$130=150.000 (x max travel, mm); for homing
$131=150.000 (y max travel, mm)
$132=30.000 (z max travel, mm)
$133...135=NA
*/


// http://www.diymachining.com/downloads/GRBL_Settings_Pocket_Guide_Rev_B.pdf
void GRBLMaster::PlatformConfig::makeDefaultConfigList0901(vector<string>& paraCodes, vector<string>& paraVals) {
	paraCodes.push_back("0");	paraVals.push_back("10");
	paraCodes.push_back("1");	paraVals.push_back("25");
	paraCodes.push_back("2");	paraVals.push_back("0");
	//paraCodes.push_back("3");	paraVals.push_back("5");
	paraCodes.push_back("4");	paraVals.push_back("0");
	paraCodes.push_back("5");	paraVals.push_back("0");
	paraCodes.push_back("6");	paraVals.push_back("0");
	paraCodes.push_back("10");	paraVals.push_back("19"); // 1+2+16
	paraCodes.push_back("11");	paraVals.push_back("0.010");
	paraCodes.push_back("12");	paraVals.push_back("0.002");
	paraCodes.push_back("13");	paraVals.push_back("0");
	paraCodes.push_back("20");	paraVals.push_back("0");
	paraCodes.push_back("21");	paraVals.push_back("1");
	paraCodes.push_back("22");	paraVals.push_back("0");
	//paraCodes.push_back("23");	paraVals.push_back("7");
	//paraCodes.push_back("24");	paraVals.push_back("");
	//paraCodes.push_back("25");	paraVals.push_back("");
	paraCodes.push_back("26");	paraVals.push_back("250");
	paraCodes.push_back("27");	paraVals.push_back("1.000");
	//paraCodes.push_back("100");	paraVals.push_back("400.000");
	//paraCodes.push_back("101");	paraVals.push_back("400.000");
	//paraCodes.push_back("102");	paraVals.push_back("400.000");
	//paraCodes.push_back("110");	paraVals.push_back("");
	//paraCodes.push_back("111");	paraVals.push_back("");
	//paraCodes.push_back("112");	paraVals.push_back("");
	//paraCodes.push_back("120");	paraVals.push_back("");
	//paraCodes.push_back("121");	paraVals.push_back("");
	//paraCodes.push_back("122");	paraVals.push_back("");
	paraCodes.push_back("130");	paraVals.push_back("999.000");
	paraCodes.push_back("131");	paraVals.push_back("999.000");
	paraCodes.push_back("132");	paraVals.push_back("999.000");
}

bool GRBLMaster::PlatformConfig::hasEntry(int codeIn) {
	for (auto& curCode : paraCodes) {
		if (curCode == codeIn) {
			return true;
		}
	}
	return false;
}

std::vector<std::vector<uint8_t>> GRBLMaster::PlatformConfig::getCmdToSetToDefaults() {
	std::vector<std::vector<uint8_t>> ret;

	std::vector<string> defaultParaCodeStrs, defaultParaValStrs;
	makeDefaultConfigList0901(defaultParaCodeStrs, defaultParaValStrs);

	for (int setIdx = 0; setIdx < defaultParaCodeStrs.size(); setIdx++) {
		for (int curConfigIdx = 0; curConfigIdx < paraCodeStrs.size(); curConfigIdx++) {
			if ((defaultParaCodeStrs[setIdx].compare(paraCodeStrs[curConfigIdx]) == 0)
				&& (defaultParaValStrs[setIdx].compare(paraValStrs[curConfigIdx]) != 0)) {
				string newCmd = "$" + defaultParaCodeStrs[setIdx] + "=" + defaultParaValStrs[setIdx] + "\r\n";
				vector<uint8_t> newCmdVec(newCmd.length());
				memcpy(&newCmdVec[0], &newCmd[0], newCmdVec.size());
				ret.push_back(newCmdVec);
			}
		}
	}

	return ret;
}

std::string GRBLMaster::PlatformConfig::toStr() {
	string ret = "";
	for (int paraIdx = 0; paraIdx < paraCodeStrs.size(); paraIdx++) {
		ret.append("$" + paraCodeStrs[paraIdx] + "=" + paraValStrs[paraIdx] + ";");
	}
	return ret;
}

void GRBLMaster::PlatformConfig::reset() {
	paraCodes.clear();
	paraCodeStrs.clear();
	//paraVals.clear();
	paraValStrs.clear();
	paraNotes.clear();
}

//
//std::string CtrlLimit2str(CtrlLimit lim) {
//	if (lim == CtrlLimit::invalidResponse) {
//		return "invalidResponse";
//	}
//	else 	if (lim == CtrlLimit::recordMismatch) {
//		return "recordMismatch";
//	}
//	else 	if (lim == CtrlLimit::maxLim) {
//		return "maxLim";
//	}
//	else 	if (lim == CtrlLimit::minLim) {
//		return "minLim";
//	}
//	else 	if (lim == CtrlLimit::highLim) {
//		return "highLim";
//	}
//	else 	if (lim == CtrlLimit::lowLim) {
//		return "lowLim";
//	}
//	else 	if (lim == CtrlLimit::withinLim) {
//		return "withinLim";
//	}
//	else {
//		return "?";
//	}
//}
//
//CtrlLimit CtrlLimitFromStr(std::string resultStr) {
//	if (resultStr.compare("invalidResponse") == 0) {
//		return CtrlLimit::invalidResponse;
//	}
//	else if (resultStr.compare("recordMismatch") == 0) {
//		return CtrlLimit::recordMismatch;
//	}
//	else if (resultStr.compare("maxLim") == 0) {
//		return CtrlLimit::maxLim;
//	}
//	else if (resultStr.compare("minLim") == 0) {
//		return CtrlLimit::minLim;
//	}
//	else if (resultStr.compare("withinLim") == 0) {
//		return CtrlLimit::withinLim;
//	}
//	return CtrlLimit::invalidResponse;
//}
//
//std::string CtrlMovState2str(CtrlMovState mov) {
//	if (mov == CtrlMovState::undefined) {
//		return "Undefined";
//	}
//	else 	if (mov == CtrlMovState::alarm) {
//		return "Alarm";
//	}
//	else 	if (mov == CtrlMovState::alarmTriggered) {
//		return "AlarmTriggered";
//	}
//	else 	if (mov == CtrlMovState::idle) {
//		return "Idle";
//	}
//	else 	if (mov == CtrlMovState::run) {
//		return "Running";
//	}
//	else 	if (mov == CtrlMovState::homing) {
//		return "Hominng";
//	}
//	else 	if (mov == CtrlMovState::retrieveFromHardLimit) {
//		return "Limited";
//	}
//	else {
//		return "?";
//	}
//}
//
//char CtrlMovState2char(CtrlMovState mov) {
//	string theStr = CtrlMovState2str(mov);
//	return theStr[0];
//}
//
//CtrlMovState CtrlMovStateFromstr(std::string resultStr) {
//	if (resultStr.compare("Undefined") == 0) {
//		return CtrlMovState::undefined;
//	}
//	else if (resultStr.compare("Alarm") == 0) {
//		return CtrlMovState::alarm;
//	}
//	else if (resultStr.compare("Idle") == 0) {
//		return CtrlMovState::idle;
//	}
//	else if (resultStr.compare("Running") == 0) {
//		return CtrlMovState::run;
//	}
//	else if (resultStr.compare("Hominng") == 0) {
//		return CtrlMovState::homing;
//	}
//	else if (resultStr.compare("Limited") == 0) {
//		return CtrlMovState::retrieveFromHardLimit;
//	}
//	return CtrlMovState::undefined;
//}
//
//bool CtrlMovStateNotMoving(CtrlMovState mov) {
//	return mov == CtrlMovState::alarm || mov == CtrlMovState::idle;
//}